/*************************************************************
*
* M3D_MATR.CPP
*
* 3D (4x4) matrix operations
*************************************************************/
#include <stdafx.h>

//#include <ap_comm.h>
#include <m3d\m3d.h>
//#include <m3d/m3d_vmx.h>
#ifdef _AP_XENON
#include <ap_comm_xenon.h>
#endif
#ifdef M3D_USE_SSE
#pragma warning(disable:4239)      // nonstandard extension used : 'argument' : conversion from 'foo' to 'foo &'
#include <fvec.h>
#endif





// check matrix accuracy; NOTE: accuracy is very low!
#define _M3D_MATR_CHECK_ACC 0.02f 

// global variables
m3dMATR m3dMatrIdentity(1.0f, 0.0f, 0.0f, 0.0f,
						0.0f, 1.0f, 0.0f, 0.0f,
						0.0f, 0.0f, 1.0f, 0.0f,
						0.0f, 0.0f, 0.0f, 1.0f);

/*************************************************************
* m3dMATR::Translate ()
* 
* IN:  pM      - 3D matrix
*      x, y, z - translation offsets
*      oper    - operation (M3D_PRE, M3D_POST, M3D_REPLACE)
* OUT: pM      - translated matrix
*************************************************************/
void m3dMATR::Translate (float x, float y, float z, int oper)
{
   switch (oper) {
      case M3D_REPLACE:
         ElementRC(3, 0) = x;
         ElementRC(3, 1) = y;
         ElementRC(3, 2) = z;
         break;
      case M3D_PRE:
         ElementRC(3, 0) += ElementRC(0, 0)*x + ElementRC(1, 0)*y + ElementRC(2, 0)*z;
         ElementRC(3, 1) += ElementRC(0, 1)*x + ElementRC(1, 1)*y + ElementRC(2, 1)*z;
         ElementRC(3, 2) += ElementRC(0, 2)*x + ElementRC(1, 2)*y + ElementRC(2, 2)*z;
         break;
      case M3D_POST:
         {
            float tx = ElementRC(3, 0);
            float ty = ElementRC(3, 1);
            float tz = ElementRC(3, 2);
            tx += x;
            ty += y;
            tz += z;
            ElementRC(3, 0) = tx;
            ElementRC(3, 1) = ty;
            ElementRC(3, 2) = tz;
         }
         break;
      default:
         ASSERT(FALSE);
         break;
   }
   return;
}
/****************************************************
* m3dMATR::MakeLCS2WCS ()
*
* Calculate local transformation matrix specifying the origin
* of the new CS and its three coord axis
*
* IN:  org - origin of LCS (specified in WCS)
*      vx, vy, vz - axis of LCS (specified in WCS)
*                   if one of this one is null, so generate it by other vectors
* OUT: pMat - computed transformation matrix
****************************************************/
void m3dMATR::MakeLCS2WCS (const m3dV *org, const m3dV *vx, const m3dV *vy, const m3dV *vz)
{
   m3dV  vStore;
#ifdef _DEBUG
   BOOL  isAllowDenormalized = TRUE; // if all 3 vectors are given - the func
   // simply places them in the matrix
   // if CrossProducts are used => all vectors need to be normalized,
   // otherwise the resultant matrix is unpredictable
#endif

   // no vX vector given...
   if (!vx)  {
      ASSERT(vy && vz);
      m3dCrossProduct(vy, vz, &vStore);
      vx = &vStore;
#ifdef _DEBUG
      isAllowDenormalized = FALSE;
#endif
   }

   // no vY vector given...
   if (!vy)  {
      ASSERT(vx && vz);
      m3dCrossProduct(vz, vx, &vStore);
      vy = &vStore;
#ifdef _DEBUG
      isAllowDenormalized = FALSE;
#endif
   }

   // no vZ vector given...
   if (!vz)  {
      ASSERT(vx && vy);
      m3dCrossProduct(vx, vy, &vStore);
      vz = &vStore;
#ifdef _DEBUG
      isAllowDenormalized = FALSE;
#endif
   }

   // check that vectors are orthonormal
   // (where rarely, but vectors may be not normal)
#ifdef _DEBUG
   if (!isAllowDenormalized) {
      ASSERT(m3dIsZero_A(m3dLengthVector(vx)-1.f,0.02f));
      ASSERT(m3dIsZero_A(m3dLengthVector(vy)-1.f,0.02f));
      ASSERT(m3dIsZero_A(m3dLengthVector(vz)-1.f,0.02f));
   }
#endif
   ASSERT(m3dIsZero_A(m3dDotProduct(vx,vy),0.02f));
   ASSERT(m3dIsZero_A(m3dDotProduct(vx,vz),0.02f));
   ASSERT(m3dIsZero_A(m3dDotProduct(vy,vz),0.02f));

   // Create transformation matrix
   ElementRC(0, 0) = vx->x;
   ElementRC(1, 0) = vy->x;
   ElementRC(2, 0) = vz->x;
   ElementRC(0, 1) = vx->y;
   ElementRC(1, 1) = vy->y;
   ElementRC(2, 1) = vz->y;
   ElementRC(0, 2) = vx->z;
   ElementRC(1, 2) = vy->z;
   ElementRC(2, 2) = vz->z;

   ElementRC(3, 0) = org->x;
   ElementRC(3, 1) = org->y;
   ElementRC(3, 2) = org->z;

   ElementRC(0, 3) = 0.0;
   ElementRC(1, 3) = 0.0;
   ElementRC(2, 3) = 0.0;
   ElementRC(3, 3) = 1.0;

   return;
}
/****************************************************
* m3dMATR::MakeWCS2LCS ()
*
* Calculate transformation matrix world->local by specifying 
* the origin and three coord axis of local CS
*
* IN:  org - origin of LCS (specified in WCS)
*      vx, vy, vz - axis of LCS (specified in WCS)
*                   if one of this one is null, so generate it by other vectors
* OUT: pMat - computed transformation matrix
****************************************************/
void m3dMATR::MakeWCS2LCS (const m3dV *org, const m3dV *vx, const m3dV *vy, const m3dV *vz)
{
   m3dV vStore;

   // no vX vector given...
   if (!vx)  {
      ASSERT(vz && vy);
      m3dCrossProduct(vy, vz, &vStore);
      vx = &vStore;
   }

   // no vY vector given...
   if (!vy)  {
      ASSERT(vx && vz);
      m3dCrossProduct(vz, vx, &vStore);
      vy = &vStore;
   }

   // no vZ vector given...
   if (!vz)  {
      ASSERT(vx && vy);
      m3dCrossProduct(vx, vy, &vStore);
      vz = &vStore;
   }

   // check that vectors are orthonormal
   // (where rarely, but vectors may be not normal)
   ASSERT(m3dIsZero_A(m3dLengthVector(vx)-1.f,0.02f));
   ASSERT(m3dIsZero_A(m3dLengthVector(vy)-1.f,0.02f));
   ASSERT(m3dIsZero_A(m3dLengthVector(vz)-1.f,0.02f));
   ASSERT(m3dIsZero_A(m3dDotProduct(vx,vy),0.02f));
   ASSERT(m3dIsZero_A(m3dDotProduct(vx,vz),0.02f));
   ASSERT(m3dIsZero_A(m3dDotProduct(vy,vz),0.02f));

   ElementRC(0, 0) = vx->x;
   ElementRC(0, 1) = vy->x;
   ElementRC(0, 2) = vz->x;
   ElementRC(0, 3) = 0.f;
   ElementRC(1, 0) = vx->y;
   ElementRC(1, 1) = vy->y;
   ElementRC(1, 2) = vz->y;
   ElementRC(1, 3) = 0.f;
   ElementRC(2, 0) = vx->z;
   ElementRC(2, 1) = vy->z;
   ElementRC(2, 2) = vz->z;
   ElementRC(2, 3) = 0.f;
   ElementRC(3, 0) = 0.f;
   ElementRC(3, 1) = 0.f;
   ElementRC(3, 2) = 0.f;
   ElementRC(3, 3) = 1.f;
   Translate(-org->x, -org->y, -org->z, M3D_PRE);

   return;
}

/*************************************************************
* m3dMATR::Transpose()
* Transpose src straight into dst to avoid redundant copying
* with the in-place version of Transpose().
*************************************************************/
/*static*/ void m3dMATR::Transpose(const m3dMATR* __RESTRICT pSrc, m3dMATR* __RESTRICT pDst)
{
#if !defined M3D_SUPPRESS_OPTIMIZATION
   vmxMATR m = vmxMatrLoadUA((float*)pSrc->elements);
   m = vmxMatrTranspose(m);
   vmxMatrStoreUA(m, pDst->elements);
#else
   pDst->ElementRC(0, 0) = pSrc->ElementRC(0, 0);
   pDst->ElementRC(0, 1) = pSrc->ElementRC(1, 0);
   pDst->ElementRC(0, 2) = pSrc->ElementRC(2, 0);
   pDst->ElementRC(0, 3) = pSrc->ElementRC(3, 0);

   pDst->ElementRC(1, 0) = pSrc->ElementRC(0, 1);
   pDst->ElementRC(1, 1) = pSrc->ElementRC(1, 1);
   pDst->ElementRC(1, 2) = pSrc->ElementRC(2, 1);
   pDst->ElementRC(1, 3) = pSrc->ElementRC(3, 1);

   pDst->ElementRC(2, 0) = pSrc->ElementRC(0, 2);
   pDst->ElementRC(2, 1) = pSrc->ElementRC(1, 2);
   pDst->ElementRC(2, 2) = pSrc->ElementRC(2, 2);
   pDst->ElementRC(2, 3) = pSrc->ElementRC(3, 2);

   pDst->ElementRC(3, 0) = pSrc->ElementRC(0, 3);
   pDst->ElementRC(3, 1) = pSrc->ElementRC(1, 3);
   pDst->ElementRC(3, 2) = pSrc->ElementRC(2, 3);
   pDst->ElementRC(3, 3) = pSrc->ElementRC(3, 3);
#endif
/*
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   vec_float4 a = vec_load_unaligned(pSrc->elements, 0);
   vec_float4 b = vec_load_unaligned(pSrc->elements, 16);
   vec_float4 c = vec_load_unaligned(pSrc->elements, 32);
   vec_float4 d = vec_load_unaligned(pSrc->elements, 48);

   vec_float4 tmp0 = vec_mergeh(a, c);
   vec_float4 tmp1 = vec_mergeh(b, d);
   vec_float4 tmp2 = vec_mergel(a, c);
   vec_float4 tmp3 = vec_mergel(b, d);

   vec_float4 r0 = vec_mergeh(tmp0, tmp1);
   vec_float4 r1 = vec_mergel(tmp0, tmp1);
   vec_float4 r2 = vec_mergeh(tmp2, tmp3);
   vec_float4 r3 = vec_mergel(tmp2, tmp3);

   vec_store_unaligned(r0, pDst->elements, 0);
   vec_store_unaligned(r1, pDst->elements, 16);
   vec_store_unaligned(r2, pDst->elements, 32);
   vec_store_unaligned(r3, pDst->elements, 48);
#elif defined _AP_PC
   __m128 mm0=_mm_loadu_ps(&pSrc->_11);
   __m128 mm1=_mm_loadu_ps(&pSrc->_21);
   __m128 mm2=_mm_loadu_ps(&pSrc->_31);
   __m128 mm3=_mm_loadu_ps(&pSrc->_41);

   _MM_TRANSPOSE4_PS(mm0, mm1, mm2, mm3);

   _mm_storeu_ps(&pDst->_11, mm0);
   _mm_storeu_ps(&pDst->_21, mm1);
   _mm_storeu_ps(&pDst->_31, mm2);
   _mm_storeu_ps(&pDst->_41, mm3);
#elif defined(_AP_XENON) && !defined(_NO_XMM)
   XMMATRIX m(pSrc->elements);
   XMMATRIX mt = XMMatrixTranspose(m);
   XMStoreFloat4x4((XMFLOAT4X4*)pDst, mt);
#else
   pDst->ElementRC(0, 0) = pSrc->ElementRC(0, 0);
   pDst->ElementRC(0, 1) = pSrc->ElementRC(1, 0);
   pDst->ElementRC(0, 2) = pSrc->ElementRC(2, 0);
   pDst->ElementRC(0, 3) = pSrc->ElementRC(3, 0);

   pDst->ElementRC(1, 0) = pSrc->ElementRC(0, 1);
   pDst->ElementRC(1, 1) = pSrc->ElementRC(1, 1);
   pDst->ElementRC(1, 2) = pSrc->ElementRC(2, 1);
   pDst->ElementRC(1, 3) = pSrc->ElementRC(3, 1);

   pDst->ElementRC(2, 0) = pSrc->ElementRC(0, 2);
   pDst->ElementRC(2, 1) = pSrc->ElementRC(1, 2);
   pDst->ElementRC(2, 2) = pSrc->ElementRC(2, 2);
   pDst->ElementRC(2, 3) = pSrc->ElementRC(3, 2);

   pDst->ElementRC(3, 0) = pSrc->ElementRC(0, 3);
   pDst->ElementRC(3, 1) = pSrc->ElementRC(1, 3);
   pDst->ElementRC(3, 2) = pSrc->ElementRC(2, 3);
   pDst->ElementRC(3, 3) = pSrc->ElementRC(3, 3);
#endif
*/
}

/****************************************************
* _m3dGetMaxAbsComponent()
*
****************************************************/
__INLINE float _m3dGetMaxAbsComponent (m3dV *v)
{
   return m3dFMax(m3dFAbs(v->x), m3dFMax(m3dFAbs(v->y), m3dFAbs(v->z)));
   /*
   if (m3dFAbs(v->x) > m3dFMax(m3dFAbs(v->y), m3dFAbs(v->z))) {
   return v->x;
   }
   if (m3dFAbs(v->y) > m3dFMax(m3dFAbs(v->x), m3dFAbs(v->z))) {
   return v->y;
   }
   return v->z;
   */
}

/**************************************************************
* m3dMATR::IsRightHanded ()
* Check if the matrix is right handed
* IN  : pMatr - matrix
* OUT : none
* RET : TRUE if the matrix right handed
**************************************************************/
bool m3dMATR::IsRightHanded (void) const
{
   m3dV zAxisProd;
   m3dCrossProduct(GetAxisX(), GetAxisY(), &zAxisProd);

   return m3dDotProduct(GetAxisZ(), &zAxisProd) > 0.f;
}



/****************************************************
* m3dMATR::GetScale()
*
* IN : pMat    - matrix
* OUT: x, y, z - scale components
*
* Return scaling components of matrix
****************************************************/
void m3dMATR::GetScale (float * __RESTRICT x, float * __RESTRICT y, float * __RESTRICT z) const
{
   bool isRightHanded;
#if defined _AP_XENON
   XMVECTOR vx = __loadunalignedvector(GetAxisX());
   XMVECTOR vy = __loadunalignedvector(GetAxisY());
   XMVECTOR vz = __loadunalignedvector(GetAxisZ());
   XMVECTOR lx = XMVector3Length(vx);
   XMVECTOR ly = XMVector3Length(vy);
   XMVECTOR lz = XMVector3Length(vz);
   __stvewx(lx, x, 0);
   __stvewx(ly, y, 0);
   __stvewx(lz, z, 0);
   XMVECTOR xcrossy = XMVector3Cross(vx, vy);
   XMVECTOR zdotz = XMVector3Dot(vz, xcrossy);
   float dot;
   __stvewx(zdotz, &dot, 0);
   isRightHanded = dot > 0.0f;
#elif defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   vec_float4 vx = vec_load_unaligned((float*)GetAxisX());
   vec_float4 vy = vec_load_unaligned((float*)GetAxisY());
   vec_float4 vz = vec_load_unaligned((float*)GetAxisZ());

   vec_float4 lx = sqrtf4(vec_dot3(vx, vx));
   vec_float4 ly = sqrtf4(vec_dot3(vy, vy));
   vec_float4 lz = sqrtf4(vec_dot3(vz, vz));

   vec_ste(lx, 0, x);
   vec_ste(ly, 0, y);
   vec_ste(lz, 0, z);

   vec_float4 xcrossy = vec_cross3(vx, vy);
   vec_float4 zdotz = vec_dot3(vz, xcrossy);
   float dot;
   vec_ste(zdotz, 0, &dot);
   isRightHanded = dot > 0.0f;
#else
   // Calculate X scale
   *x = m3dLengthVector(GetAxisX());
   // Calculate Y scale
   *y = m3dLengthVector(GetAxisY());
   // Calculate Z scale
   *z = m3dLengthVector(GetAxisZ());
   isRightHanded = IsRightHanded();
#endif
   if (!isRightHanded) {
      // some more clever processing could be required in this case
      // what if only one axis is inverted?
      if (_m3dGetMaxAbsComponent(GetAxisX()) < 0.f) {
         *x *= -1.f;
      }
      if (_m3dGetMaxAbsComponent(GetAxisY()) < 0.f) {
         *y *= -1.f;
      }
      if (_m3dGetMaxAbsComponent(GetAxisZ()) < 0.f) {
         *z *= -1.f;
      }
      /*
      (*x) *= -1.f;
      (*y) *= -1.f;
      (*z) *= -1.f;
      */
   } 

   return;
}

/********************************************************************
* m3dInit3DMatr ()
********************************************************************/
BOOL m3dInit3DMatr (void)
{
   return TRUE;
}
/********************************************************************
* m3dTerm3DMatr ()
********************************************************************/
void m3dTerm3DMatr (void)
{
   return;
}

/*************************************************************
* m3dMATR related functions
*************************************************************/
/*************************************************************
* m3dAddMatr()
* 
* Add 3D matrices: c = a + b
*
* IN:  a, b - 3D matrices
* OUT: c    - 3D matrix
*************************************************************/
void m3dAddMatr (const m3dMATR *a, const m3dMATR *b, m3dMATR *c)
{
   int i;

   // c[row][col] = a[row][col] + b[row][col]
   // loop is the linear form of double-loop
   for (i = 0; i < 16; i++)  {
      m3dMatrElement(c,i) = m3dMatrElement(a,i) + m3dMatrElement(b,i);
   }
   return;
}
/*************************************************************
* m3dSubtractMatr()
* 
* Subtract 3D matrices: c = a - b
*
* IN:  a, b - 3D matrices
* OUT: c    - 3D matrix
*************************************************************/
void m3dSubtractMatr (const m3dMATR *a, const m3dMATR *b, m3dMATR *c)
{
   int i;

   // c[row][col] = a[row][col] - b[row][col]
   // loop is the linear form of double-loop
   for (i = 0; i < 16; i++)  {
      m3dMatrElement(c,i) = m3dMatrElement(a,i) - m3dMatrElement(b,i);
   }
   return;
}
/*************************************************************
* m3dMultiplyMatr()
* 
* Multiply 3D matrices: c = a * b
*
* IN:  a, b - 3D matrices
* OUT: c    - 3D matrix
*
*************************************************************/
void m3dMultiplyMatr (const m3dMATR * __RESTRICT a, const m3dMATR * __RESTRICT b, m3dMATR * __RESTRICT c)
{
#if !defined M3D_SUPPRESS_OPTIMIZATION
   vmxMATR _ma = vmxMatrLoadUA((float*)a->elements);
   vmxMATR _mb = vmxMatrLoadUA((float*)b->elements);
   vmxMATR _mc = _ma * _mb;
   vmxMatrStoreUA(_mc, c->elements);
#else //CRT/FPU

   int       row, col;
   m3dMATR   tmp, *_c;

   _c = c;
   if (a == c || b == c) {
      _c  = &tmp;
   }
   for (row = 0; row < 4; row++)  {
      for (col = 0; col < 4; col++)  {
         m3dMatrElementRC(_c,row,col) = 
            m3dMatrElementRC(a,row,0) * m3dMatrElementRC(b,0,col) + 
            m3dMatrElementRC(a,row,1) * m3dMatrElementRC(b,1,col) + 
            m3dMatrElementRC(a,row,2) * m3dMatrElementRC(b,2,col) + 
            m3dMatrElementRC(a,row,3) * m3dMatrElementRC(b,3,col);
      }
   }
   if (a == c || b == c) {
      *c = tmp;
   }
#endif
/*

#if defined(_AP_XENON) && !defined(_NO_XMM)
   XMMATRIX ma(a->elements);
   XMMATRIX mb(b->elements);
   XMMATRIX mc = ma * mb;
   XMStoreFloat4x4((XMFLOAT4X4*)c, mc);
   return;
#elif defined(_AP_PS3) && !defined M3D_SUPPRESS_OPTIMIZATION
   vec_float4 maa = vec_load_unaligned(a->elements, 0);
   vec_float4 mab = vec_load_unaligned(a->elements, 16);
   vec_float4 mac = vec_load_unaligned(a->elements, 32);
   vec_float4 mad = vec_load_unaligned(a->elements, 48);

   vec_float4 mba = vec_load_unaligned(b->elements, 0);
   vec_float4 mbb = vec_load_unaligned(b->elements, 16);
   vec_float4 mbc = vec_load_unaligned(b->elements, 32);
   vec_float4 mbd = vec_load_unaligned(b->elements, 48);

	vec_float4 mca = MxV(mba, mbb, mbc, mbd, maa);
	vec_float4 mcb = MxV(mba, mbb, mbc, mbd, mab);
	vec_float4 mcc = MxV(mba, mbb, mbc, mbd, mac);
	vec_float4 mcd = MxV(mba, mbb, mbc, mbd, mad);

   vec_store_unaligned(mca, c->elements,  0);
   vec_store_unaligned(mcb, c->elements, 16);
   vec_store_unaligned(mcc, c->elements, 32);
   vec_store_unaligned(mcd, c->elements, 48);

	return;
#elif defined M3D_USE_SSE
   F32vec4 Result;
   F32vec4 B1 = _mm_loadu_ps(&b->_11);
   F32vec4 B2 = _mm_loadu_ps(&b->_21);
   F32vec4 B3 = _mm_loadu_ps(&b->_31);
   F32vec4 B4 = _mm_loadu_ps(&b->_41);

   Result  = F32vec4(a->_11) * B1;
   Result += F32vec4(a->_12) * B2;
   Result += F32vec4(a->_13) * B3;
   Result += F32vec4(a->_14) * B4;
   _mm_storeu_ps(&c->_11, Result);

   Result  = F32vec4(a->_21) * B1;
   Result += F32vec4(a->_22) * B2;
   Result += F32vec4(a->_23) * B3;
   Result += F32vec4(a->_24) * B4;
   _mm_storeu_ps(&c->_21, Result);

   Result  = F32vec4(a->_31) * B1;
   Result += F32vec4(a->_32) * B2;
   Result += F32vec4(a->_33) * B3;
   Result += F32vec4(a->_34) * B4;
   _mm_storeu_ps(&c->_31, Result);

   Result  = F32vec4(a->_41) * B1;
   Result += F32vec4(a->_42) * B2;
   Result += F32vec4(a->_43) * B3;
   Result += F32vec4(a->_44) * B4;
   _mm_storeu_ps(&c->_41, Result);

#else //CRT/FPU

   int       row, col;
   m3dMATR   tmp, *_c;

   _c = c;
   if (a == c || b == c) {
      _c  = &tmp;
   }
   for (row = 0; row < 4; row++)  {
      for (col = 0; col < 4; col++)  {
         m3dMatrElementRC(_c,row,col) = 
            m3dMatrElementRC(a,row,0) * m3dMatrElementRC(b,0,col) + 
            m3dMatrElementRC(a,row,1) * m3dMatrElementRC(b,1,col) + 
            m3dMatrElementRC(a,row,2) * m3dMatrElementRC(b,2,col) + 
            m3dMatrElementRC(a,row,3) * m3dMatrElementRC(b,3,col);
      }
   }
   if (a == c || b == c) {
      *c = tmp;
   }
#endif
*/
   return;
}


/*************************************************************
* m3dMultiplyMatrAccuracy()
* 
* Multiply 3D matrices: c = a * b
*
* IN:  a, b - 3D matrices
* OUT: c    - 3D matrix
*
* Skolunov: Get more accurace result than m3dMultiplyMatr(), not use SIMD
*************************************************************/
void m3dMultiplyMatrAccuracy(const m3dMATR * __RESTRICT a, const m3dMATR * __RESTRICT b, m3dMATR * __RESTRICT c)
{
   int       row, col;
   m3dMATR   tmp, * __RESTRICT _c;

   _c = c;
   if (a == c || b == c) {
      _c  = &tmp;
   }
   for (row = 0; row < 4; row++)  {
      for (col = 0; col < 4; col++)  {
         m3dMatrElementRC(_c,row,col) = 
            m3dMatrElementRC(a,row,0) * m3dMatrElementRC(b,0,col) + 
            m3dMatrElementRC(a,row,1) * m3dMatrElementRC(b,1,col) + 
            m3dMatrElementRC(a,row,2) * m3dMatrElementRC(b,2,col) + 
            m3dMatrElementRC(a,row,3) * m3dMatrElementRC(b,3,col);
      }
   }
   if (a == c || b == c) {
      *c = tmp;
   }

   return;
}
/******************************************************************
* m3dIsEqualMatr_A()
*
* IN: pMat1, pMat2    - matrices to compare
*     accRot, accTran - accuracy (rotational / translational)
*
* OUT: TRUE  - if matrices are equivalent with given accuracies
*      FALSE - otherwise
*******************************************************************/
BOOL m3dIsEqualMatr_A (const m3dMATR *pMat1, const m3dMATR *pMat2, float accRot, float accTran)
{
   int r,c;

   for (r = 0; r < 3; r++) {
      for (c = 0; c < 4; c++) {
         if (m3dFAbs(m3dMatrElementRC(pMat1, r, c) - m3dMatrElementRC(pMat2, r, c)) > accRot) {
            return FALSE;
         }
      }
   }
   r = 3;
   for (c = 0; c < 4; c++) {
      if (m3dFAbs(m3dMatrElementRC(pMat1, r, c) - m3dMatrElementRC(pMat2, r, c)) > accTran) {
         return FALSE;
      }
   }
   return TRUE;
}
/*************************************************************
* m3dDivideMatr ()
*
*    Divide one matrix by another
*************************************************************/
void m3dDivideMatr (const m3dMATR *a, const m3dMATR *b, m3dMATR *c)
{
   m3dMATR  d;

   b->Invert(&d);
   m3dMultiplyMatr(a, &d, c);
   return;
}

/*************************************************************
* m3dDivideMatrLeft ()
*
*    Divide one matrix by another
*************************************************************/
void m3dDivideMatrLeft (const m3dMATR *a, const m3dMATR *b, m3dMATR *c)
{
   m3dMATR  d;

   a->Invert(&d);
   m3dMultiplyMatr(&d, b, c);
   return;
}

/*************************************************************
* m3dMATR class members
*************************************************************/


/*************************************************************
* m3dMATR::Identity ()
*
* Make 3D Identity matrix
*
* OUT:  pM - matrix to be made identity
*************************************************************/
void m3dMATR::Identity (void)
{
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
	// This version is 25-30% faster than the version below.
	const unsigned int one = 0x3f800000;	// 1.0f in hex.
	// Use __builtin_memset to avoid unnecessary stores.
	__builtin_memset(this, 0, sizeof(m3dMATR));
	unsigned int * elementsi = (unsigned int *)elements;
	elementsi[0] = one;
	elementsi[5] = one;
	elementsi[10] = one;
	elementsi[15] = one;
#elif defined _AP_XENON
	const unsigned int one = 0x3f800000;
	unsigned int *elementsi = (unsigned int *)elements;
	elementsi[1] = elementsi[2] = elementsi[3] = elementsi[4] = 0;
	elementsi[6] = elementsi[7] = elementsi[8] = elementsi[9] = 0;
	elementsi[11] = elementsi[12] = elementsi[13] = elementsi[14] = 0;
	elementsi[0] = elementsi[5] = elementsi[10] = elementsi[15] = one;
#else
   *this = m3dMatrIdentity;

   memset(this,0,sizeof(m3dMATR));
   for (int i = 0; i < 4; i++ )  {
      ElementRC(i,i) = 1.f;
   }
   return;
#endif
}


/*****************************************************
* m3dMATR::IsIdentity()
*
*****************************************************/
bool m3dMATR::IsIdentity(void) const
{
   for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 4; j++) {
         if (!m3dIsEqual(m[i][j], (i == j) ? 1.0f : 0.0f)) {
            return false;
         }
      }
   }
   return true;
}

/*************************************************************
* m3dMATR::Translate ()
* 
* IN:  pM      - 3D matrix
*      v       - translation position
*      oper    - operation (M3D_PRE, M3D_POST, M3D_REPLACE)
* OUT: pM      - translated matrix
*************************************************************/
void m3dMATR::Translate (const m3dV *v, int oper)
{
   Translate(v->x, v->y, v->z, oper);
   return;
}
/*************************************************************
* m3dMATR::RotateX()
* m3dMATR::RotateY()
* m3dMATR::RotateZ()
*
* Rotate matrix around X axis by 'angle' degrees
*
* IN:  pM    - matrix
*      angle - angle of rotation (in degrees)
*      oper  - operation (M3D_PRE, M3D_POST)
* OUT: pM    - rotated matrix
*************************************************************/
void m3dMATR::RotateX (float angle, int oper)
{
   float  cosA, sinA;

   // REPLACE is not valid for rotations
   ASSERT(oper == M3D_PRE || oper == M3D_POST);

   angle = m3dDeg2Rad(angle);

   m3dSinCos(angle, &sinA, &cosA);

   m3dMATR m3dMatrRotX;
   m3dMatrRotX.Identity();

   // construct rotX transformation matrix
   m3dMatrRotX.ElementRC(1, 1) = cosA;
   m3dMatrRotX.ElementRC(1, 2) = sinA;
   m3dMatrRotX.ElementRC(2, 1) = -sinA;
   m3dMatrRotX.ElementRC(2, 2) = cosA;

   Transform(&m3dMatrRotX, oper);

   return;
}
void m3dMATR::RotateY (float angle, int oper)
{
   float  cosA, sinA;

   // REPLACE is not valid for rotations
   ASSERT(oper == M3D_PRE || oper == M3D_POST);

   angle = m3dDeg2Rad(angle);

   m3dSinCos(angle, &sinA, &cosA);

   m3dMATR m3dMatrRotY;
   m3dMatrRotY.Identity();

   // construct rotY transformation matrix
   m3dMatrRotY.ElementRC(0, 0) = cosA;
   m3dMatrRotY.ElementRC(0, 2) = -sinA;
   m3dMatrRotY.ElementRC(2, 0) = sinA;
   m3dMatrRotY.ElementRC(2, 2) = cosA;

   Transform(&m3dMatrRotY, oper);

   return;
}
void m3dMATR::RotateZ (float angle, int oper)
{
   float  cosA, sinA;

   // REPLACE is not valid for rotations
   ASSERT(oper == M3D_PRE || oper == M3D_POST);

   angle = m3dDeg2Rad(angle);

   m3dSinCos(angle, &sinA, &cosA);

   m3dMATR m3dMatrRotZ;
   m3dMatrRotZ.Identity();

   // construct rotX transformation matrix
   m3dMatrRotZ.ElementRC(0, 0) = cosA;
   m3dMatrRotZ.ElementRC(0, 1) = sinA;
   m3dMatrRotZ.ElementRC(1, 0) = -sinA;
   m3dMatrRotZ.ElementRC(1, 1) = cosA;

   Transform(&m3dMatrRotZ, oper);

   return;
}
/*************************************************************
* m3dMATR::Scale()
* 
* Scale 3D matrix
*
* IN:  pM            - 3D matrix
*      sx, sy, sz    - scale factors
*      oper          - operation (M3D_PRE, M3D_POST, M3D_REPLACE)
* OUT: pM            - scaled 3D matrix
*************************************************************/
void m3dMATR::Scale (float sx, float sy, float sz, int oper)
{
   int  i;

   ASSERT(oper == M3D_REPLACE || oper == M3D_PRE || oper == M3D_POST);

   if (oper == M3D_REPLACE) {
      oper = M3D_POST;
      RemoveScale();
   }

   switch (oper) {
      case M3D_PRE:
         for (i=0; i<3; i++) {
			 float x = ElementRC(0, i);
			 float y = ElementRC(1, i);
			 float z = ElementRC(2, i);
			 x *= sx;
			 y *= sy;
			 z *= sz;
			 ElementRC(0,i) = x;
			 ElementRC(1,i) = y;
			 ElementRC(2,i) = z;
         }
         break;
      case M3D_POST:
         for (i=0; i<4; i++) {
			 float x = ElementRC(i, 0);
			 float y = ElementRC(i, 1);
			 float z = ElementRC(i, 2);
			 x *= sx;
			 y *= sy;
			 z *= sz;
			 ElementRC(i, 0) = x;
			 ElementRC(i, 1) = y;
			 ElementRC(i, 2) = z;
         }
         break;
   }
   return;
}
/*************************************************************
* m3dMATR::Transform ()
*
* General 3D matrix transformation
*
* IN:  pM      - matrix to be transformed
*      pMTrans - transformation matrix
*      oper    - operation (M3D_PRE, M3D_POST, M3D_REPLACE)
* OUT: pM      - transformed matrix
*************************************************************/
void m3dMATR::Transform(const m3dMATR *pMTrans, int oper)
{
   m3dMATR  matrTmp;

   ASSERT(oper == M3D_REPLACE || oper == M3D_PRE || oper == M3D_POST);

   if (oper == M3D_REPLACE)  {
      *this = *pMTrans; 
   } else if (oper == M3D_POST)  {
      m3dMultiplyMatr(this, pMTrans, &matrTmp);
      *this = matrTmp;
   } else { // oper == M3D_PRE
      m3dMultiplyMatr(pMTrans, this, &matrTmp);
      *this = matrTmp;
   }

   return;
}

/****************************************************
 * m3dMATR::GetRotateRot()
 *
 *  Get rotation component of rotation matrix
 *      (See Watt-Watt 356 p.)
 *
 * Parameters: pointer to matrix
 *             pointers to store rotation components
 * Return:     rotate X, rotate Y, rotate Z (in degrees)
 ****************************************************/
void m3dMATR::GetRotateRot(float *rotX, float *rotY, float *rotZ) const
{
   float   sinY, rx_rz;

   sinY = ElementRC(0,2);
   if(m3dFAbs(1.f-m3dFAbs(sinY)) > M3D_EPSILON ) {
      // simple case See Watt-Watt 356 p.
      *rotY = m3dRad2Deg((float)m3dASin(_m3dClamp (-1.f, 1.f, -sinY )));
      *rotX = m3dRad2Deg((float)m3dATan2( ElementRC(1,2), ElementRC(2,2) ));
      *rotZ = m3dRad2Deg((float)m3dATan2( ElementRC(0,1), ElementRC(0,0) ));
   } else {
      // "Gimbal lock" case See Watt-Watt 357 p.
      *rotY = m3dRad2Deg((float)(M3D_PI/2.f*m3dSign(-sinY)));
      rx_rz = m3dRad2Deg((float)m3dATan2(ElementRC(1,0), ElementRC(1,1)));
      *rotX = 0;
      *rotZ = *rotX-rx_rz;
   }
   return;
}
/****************************************************
 * m3dMATR::GetRotate()
 *
 *  Get rotation companent of transformation matrix
 *      (See Watt-Watt 356 p.)
 *
 * Parameters: pointer to matrix
 *             pointers to store rotation components
 * Return:     rotate X, rotate Y, rotate Z (in degrees)
 ****************************************************/
void m3dMATR::GetRotate (float *rotX, float *rotY, float *rotZ) const
{
   m3dMATR rotMatr;
   float   isx,isy,isz;

   GetInvScale(&isx, &isy, &isz);

   rotMatr = *this;
   rotMatr.Scale(isx, isy, isz, M3D_PRE);
   rotMatr.GetRotateRot(rotX, rotY, rotZ);

//#ifdef _DEBUG
//   {
//      m3dMATR m1;
//      float   tx,ty,tz;
//
//      rotMatr.GetTranslate(&tx, &ty, &tz);
//      m1.MakeTRS(tx, ty, tz, *rotX, *rotY, *rotZ, sx, sy, sz);
//      if (!m3dIsEqualMatr_A(this, &m1, 1000*M3D_EPSILON, 1000*M3D_EPSILON)) {
//         ASSERT(FALSE);
//      }
//   }
//#endif
   return;
}

/****************************************************
* m3dMATR::GetScaleX()
*
* IN : pMat    - matrix
* OUT: x       - scale components
*
* Return x scaling component of matrix
****************************************************/
void m3dMATR::GetScaleX (float * __RESTRICT x) const
{
	bool isRightHanded;
#if defined _AP_XENON
	XMVECTOR vx = __loadunalignedvector(GetAxisX());
	XMVECTOR vy = __loadunalignedvector(GetAxisY());
	XMVECTOR vz = __loadunalignedvector(GetAxisZ());
	XMVECTOR lx = XMVector3Length(vx);
	__stvewx(lx, x, 0);
	XMVECTOR xcrossy = XMVector3Cross(vx, vy);
	XMVECTOR zdotz = XMVector3Dot(vz, xcrossy);
	float dot;
	__stvewx(zdotz, &dot, 0);
	isRightHanded = dot > 0.0f;
#elif defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   vec_float4 vx = vec_load_unaligned((float*)GetAxisX());
   vec_float4 vy = vec_load_unaligned((float*)GetAxisY());
   vec_float4 vz = vec_load_unaligned((float*)GetAxisZ());

   vec_float4 lx = sqrtf4(vec_dot3(vx, vx));

   vec_ste(lx, 0, x);

   vec_float4 xcrossy = vec_cross3(vx, vy);
   vec_float4 zdotz = vec_dot3(vz, xcrossy);
   float dot;
   vec_ste(zdotz, 0, &dot);
   isRightHanded = dot > 0.0f;
#else
   // Calculate X scale
   *x = m3dLengthVector(GetAxisX());
   isRightHanded = IsRightHanded();
#endif
   if (!isRightHanded) {
      // some more clever processing could be required in this case
      // what if only one axis is inverted?
      if (_m3dGetMaxAbsComponent(GetAxisX()) < 0.f) {
         *x *= -1.f;
      }
   } 

   return;
}


/****************************************************
* m3dMATR::GetInvScale()
*
* IN : pMat    - matrix
* OUT: x, y, z - inverse scale components
*
* Return inverse (reciprocal) of the scaling components of matrix
****************************************************/
void m3dMATR::GetInvScale (float * __RESTRICT x, float * __RESTRICT y, float * __RESTRICT z) const
{
	bool isRightHanded;
#if defined _AP_XENON
	XMVECTOR vx = __loadunalignedvector(GetAxisX());
	XMVECTOR vy = __loadunalignedvector(GetAxisY());
	XMVECTOR vz = __loadunalignedvector(GetAxisZ());
	XMVECTOR lx = XMVector3ReciprocalLength(vx);
	XMVECTOR ly = XMVector3ReciprocalLength(vy);
	XMVECTOR lz = XMVector3ReciprocalLength(vz);
	__stvewx(lx, x, 0);
	__stvewx(ly, y, 0);
	__stvewx(lz, z, 0);
	XMVECTOR xcrossy = XMVector3Cross(vx, vy);
	XMVECTOR zdotz = XMVector3Dot(vz, xcrossy);
	float dot;
	__stvewx(zdotz, &dot, 0);
	isRightHanded = dot > 0.0f;
#elif defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   vec_float4 vx = vec_load_unaligned((float*)GetAxisX());
   vec_float4 vy = vec_load_unaligned((float*)GetAxisY());
   vec_float4 vz = vec_load_unaligned((float*)GetAxisZ());

   vec_float4 lx = rsqrtf4(vec_dot3(vx, vx)); // vec_rsqrte is too inaccurate
   vec_float4 ly = rsqrtf4(vec_dot3(vy, vy));
   vec_float4 lz = rsqrtf4(vec_dot3(vz, vz));

   vec_ste(lx, 0, x);
   vec_ste(ly, 0, y);
   vec_ste(lz, 0, z);

   vec_float4 xcrossy = vec_cross3(vx, vy);
   vec_float4 zdotz = vec_dot3(vz, xcrossy);
   float dot;
   vec_ste(zdotz, 0, &dot);
   isRightHanded = dot > 0.0f;
#else
   // Calculate X scale
   *x = 1.0f / m3dLengthVector(GetAxisX());
   // Calculate Y scale
   *y = 1.0f / m3dLengthVector(GetAxisY());
   // Calculate Z scale
   *z = 1.0f/ m3dLengthVector(GetAxisZ());
   isRightHanded = IsRightHanded();
#endif
   if (!isRightHanded) {
      // some more clever processing could be required in this case
      // what if only one axis is inverted?
      if (_m3dGetMaxAbsComponent(GetAxisX()) < 0.f) {
         *x *= -1.f;
      }
      if (_m3dGetMaxAbsComponent(GetAxisY()) < 0.f) {
         *y *= -1.f;
      }
      if (_m3dGetMaxAbsComponent(GetAxisZ()) < 0.f) {
         *z *= -1.f;
      }
/*
      (*x) *= -1.f;
      (*y) *= -1.f;
      (*z) *= -1.f;
*/
   } 

   return;
}


/****************************************************
* m3dMATR::IsNotUnitScale()
*
**********************************************************************/
bool m3dMATR::IsNotUnitScale ()
{
   if (!IsRightHanded()) {
      return true;
   }
   float x, y, z;
   GetScale(&x, &y, &z);
   if (m3dFAbs(x-1.f)>0.0001f || m3dFAbs(y-1.f)>0.0001f || m3dFAbs(z-1.f)>0.0001f) {
      return true;
   }   
   return false;
}

/**********************************************************************
* m3dMATR::MakeTranslate ()
*
* Make translation matrix
**********************************************************************/
void m3dMATR::MakeTranslate (float  x, float  y, float z)
{
   Identity();

   ElementRC(3, 0) = x;
   ElementRC(3, 1) = y;
   ElementRC(3, 2) = z;

   return;
}
/**********************************************************************
* m3dMATR::MakeScale ()
*
* Make scale matrix. 
**********************************************************************/
void m3dMATR::MakeScale (float sx, float sy, float sz)
{
   memset(this,0,sizeof(m3dMATR));

   ElementRC(0,0) = sx;
   ElementRC(1,1) = sy;
   ElementRC(2,2) = sz;
   ElementRC(3,3) = 1.f;
   
   return;
}


/**********************************************************************
* m3dMATR::MakeRotX()
*
* Make rotation matrix. Rotation around X by 'angle' degrees
*
* IN :  angle    - angle of rotation (in degrees)
* OUT:  matrRot  - matrix
**********************************************************************/
void m3dMATR::MakeRotX (float angle)
{
   float    cosA, sinA;

   memset(this, 0, sizeof(m3dMATR));

   angle = m3dDeg2Rad(angle);

   m3dSinCos(angle, &sinA, &cosA);

   // construct rotX transformation matrix
   ElementRC(1, 1) =  cosA;
   ElementRC(1, 2) =  sinA;
   ElementRC(2, 1) = -sinA;
   ElementRC(2, 2) =  cosA;
   ElementRC(0, 0) =  1.f;
   ElementRC(3, 3) =  1.f;

   return;
}
/**********************************************************************
* m3dMATR::MakeRotY()
*
* Make rotation matrix. Rotation around Y by 'angle' degrees
*
* IN :  angle    - angle of rotation (in degrees)
* OUT:  matrRot  - matrix
**********************************************************************/
void m3dMATR::MakeRotY (float angle)
{
   float    cosA, sinA;

   memset(this, 0, sizeof(m3dMATR));

   angle = m3dDeg2Rad(angle);

   m3dSinCos(angle, &sinA, &cosA);

   // construct rotY transformation matrix
   ElementRC(0, 0) =  cosA;
   ElementRC(0, 2) = -sinA;
   ElementRC(2, 0) =  sinA;
   ElementRC(2, 2) =  cosA;
   ElementRC(1, 1) =  1.f;
   ElementRC(3, 3) =  1.f;

   return;
}
/**********************************************************************
* m3dMATR::MakeRotZ()
*
* Make rotation matrix. Rotation around Z by 'angle' degrees
*
* IN :  angle    - angle of rotation (in degrees)
* OUT:  matrRot  - matrix
**********************************************************************/
void m3dMATR::MakeRotZ (float angle)
{
   float cosA, sinA;

   memset(this, 0, sizeof(m3dMATR));

   angle = m3dDeg2Rad(angle);

   m3dSinCos(angle, &sinA, &cosA);

   // construct rotZ transformation matrix
   ElementRC(0, 0) =  cosA;
   ElementRC(0, 1) =  sinA;
   ElementRC(1, 0) = -sinA;
   ElementRC(1, 1) =  cosA;
   ElementRC(2, 2) =  1.f;
   ElementRC(3, 3) =  1.f;

   return;
} 
/*************************************************************
* m3dMATR::MakeRot()
*
* Calculate "smallest rotation" matrix that transforms one vector 
* to another
*************************************************************/
void m3dMATR::MakeRot (const m3dV *norm1, const m3dV *norm2)
{
   float    angle;
   m3dV     axis, n1, n2;
   m3dQUAT  quat;

   n1 = *norm1;
   n2 = *norm2;
   m3dNormalize(&n1);
   m3dNormalize(&n2);
   m3dCrossProduct(&n1, &n2, &axis);
   
   if (m3dIsVZero(&axis)) {
      ASSERT(m3dIsEqualPoint_A(&n1, &n2, 10.f * M3D_EPSILON));
      Identity();
      return;
   }

   m3dNormalize(&axis);
   angle = _m3dAngleVector(&n1, &n2);

   quat.SetAngAxis(angle, &axis);
   quat.GetMatr(this);

   return;
}

/*************************************************************
* m3dMATR::MakeRot()
*
* rotate from norm1 to norm2 by angle degrees
*************************************************************/
void m3dMATR::MakeRot (const m3dV *norm1, const m3dV *norm2, float angle)
{
   m3dV axis;
   m3dCrossProduct(norm1, norm2, &axis);

   if (m3dIsVZero(&axis)) {
      Identity();
      return;
   } else {
      m3dNormalize(&axis);
   }

   m3dQUAT  quat;
   quat.SetAngAxis(angle, &axis);
   quat.GetMatr(this);

   return;
}

/*************************************************************
* m3dMATR::MakeRot()
*
* Calculate "smallest rotation" matrix that transforms one vector 
* to another around origin...
*************************************************************/
void m3dMATR::MakeRot (const m3dV *org, const m3dV *norm1, const m3dV *norm2)
{
   MakeRot(norm1, norm2);
   // add translations
   Translate(-org->x, -org->y, -org->z, M3D_PRE);
   Translate( org->x,  org->y,  org->z, M3D_POST);

   return;
}

/*************************************************************
* m3dMATR::MakeRotRay ()
*
* Calculate matrix that makes rotation around a ray
*
* IN  : org / dir - ray
*       angle     - rotation angle
* OUT : matr      - pointer to rotation matrix
*************************************************************/
void m3dMATR::MakeRotRay (m3dV const *org, m3dV const *dir, float angle)
{
   m3dQUAT quat;

   // create rotation matrix
   quat.SetAngAxis(angle, dir);
   quat.GetMatr(this);

   // add translations
   Translate(-org->x, -org->y, -org->z, M3D_PRE);
   Translate( org->x,  org->y,  org->z, M3D_POST);

   return;
}

/****************************************************
* m3dMATR::_MakeWCS2LCS_VY()
*
****************************************************/
void m3dMATR::_MakeWCS2LCS_VY (m3dV *org, m3dV *dir) 
{
   m3dV  vz, vy, vzNC;

   ASSERT(m3dLengthVector(dir) > 100.f*M3D_EPSILON);

   vy = *dir;
   m3dNormalize(&vy);
   if ((m3dFAbs(vy.x) < M3D_EPSILON) && (m3dFAbs(vy.z) < M3D_EPSILON))   {
      // then vz has direction of oY axis
      vzNC = m3dVUnitX;
   }  else   {
      vzNC = m3dVUnitY;
   }
   m3dTangentComponent(&vzNC, &vy, &vz);
   m3dNormalize(&vz);
   MakeWCS2LCS(org, NULL, &vy, &vz);
   return;
}

/****************************************************
* m3dMATR::_MakeWCS2LCS_VZ()
*
****************************************************/
void m3dMATR::_MakeWCS2LCS_VZ (m3dV *org, m3dV *dir) 
{
   m3dV  vy, vz, vyNC;

   ASSERT(m3dLengthVector(dir) > 100.f*M3D_EPSILON);

   vz = *dir;
   m3dNormalize(&vz);
   if ((m3dFAbs(vz.x) < M3D_EPSILON) && (m3dFAbs(vz.z) < M3D_EPSILON))   {
      // then vz has direction of oY axis
      vyNC = m3dVUnitX;
   }  else   {
      vyNC = m3dVUnitY;
   }
   m3dTangentComponent(&vyNC, &vz, &vy);
   m3dNormalize(&vy);
   MakeWCS2LCS(org, NULL, &vy, &vz);
   return;
}

/****************************************************
* m3dMATR::MakeLCS2WCS_VZ ()
*
* Calculate local transformation matrix specifying the origin
* of the new CS and its VZ axis. The other two axes
* are computed by default.
*
* IN:  org  - origin of LCS (specified in WCS)
*      vz   - axis of LCS (specified in WCS); this vector
*             is projected onto horizontal plane (vz.y = 0)
* OUT: pMat - computed transformation matrix
****************************************************/
void m3dMATR::MakeLCS2WCS_VZ (m3dV *org, m3dV *dir) 
{
   m3dV  vz;

   vz   = *dir;
   vz.y = 0.f;

   if (!_m3dNormalize(&vz)) {
      vz = m3dVUnitX;
      //ASSERT(FALSE);
   }
   MakeLCS2WCS(org, NULL, &m3dVUnitY, &vz);
   return;
}

/****************************************************
* m3dMATR::_MakeLCS2WCS_VX()
*
* Calculate local transformation matrix specifying the origin
* of the new CS and its VX axis. The other two axes
* are computed by default.
*
* IN:  org  - origin of LCS (specified in WCS)
*      vx   - axis of LCS (specified in WCS)
* OUT: pMat - computed transformation matrix
****************************************************/
void m3dMATR::_MakeLCS2WCS_VX (m3dV *org, m3dV *dir) 
{
   m3dV  vy, vx, vyNC;

   ASSERT(m3dLengthVector(dir) > 100.f*M3D_EPSILON);

   vx = *dir;
   m3dNormalize(&vx);
   if (m3dFAbs(m3dDotProduct(&m3dVUnitY, &vx)) > 0.5f)   {
      vyNC = m3dVUnitX;
   } else {
      vyNC = m3dVUnitY;
   }
   m3dTangentComponent(&vyNC, &vx, &vy);
   m3dNormalize(&vy);
   MakeLCS2WCS(org, &vx, &vy, NULL);
   return;
}

/****************************************************
* m3dMATR::_MakeLCS2WCS_VZ ()
*
* Calculate local transformation matrix specifying the origin
* of the new CS and its VZ axis. The other two axes
* are computed by default.
*
* IN:  org  - origin of LCS (specified in WCS)
*      vz   - axis of LCS (specified in WCS)
* OUT: pMat - computed transformation matrix
****************************************************/
void m3dMATR::_MakeLCS2WCS_VZ (m3dV *org, m3dV *dir) 
{
   m3dV  vy, vz, vyNC;

   ASSERT(m3dLengthVector(dir) > 100.f*M3D_EPSILON);

   vz = *dir;
   m3dNormalize(&vz);
   if (m3dFAbs(m3dDotProduct(&m3dVUnitY, &vz)) > 0.5f)   {
      // then vz has direction of oY axis
      vyNC = m3dVUnitX;
   }  else   {
      vyNC = m3dVUnitY;
   }
   m3dTangentComponent(&vyNC, &vz, &vy);
   m3dNormalize(&vy);
   MakeLCS2WCS(org, NULL, &vy, &vz);
   return;
}

/****************************************************
* m3dMATR::_MakeLCS2WCS_VY ()
*
* IN:  org - origin of LCS (specified in WCS)
* OUT: pMat - computed transformation matrix
* NOTE: pMat maps LCS -> WCS
****************************************************/
void m3dMATR::_MakeLCS2WCS_VY (m3dV *org, m3dV *dir) 
{
   m3dV  vx, vy, vxNC;

   ASSERT(m3dLengthVector(dir) > M3D_EPSILON);

   vy = *dir;
   m3dNormalize(&vy);
   if (m3dFAbs(m3dDotProduct(&m3dVUnitY, &vy)) > 0.5f)   {
      // then vy has direction of oY axis
      vxNC = m3dVUnitX;
   }  else   {
      vxNC = m3dVUnitY;
   }
   m3dTangentComponent(&vxNC, &vy, &vx);
   m3dNormalize(&vx);
   MakeLCS2WCS(org, &vx, &vy, NULL);
   return;
}

/****************************************************
* m3dMATR::MakeTRS()
*
* Calculate transformation matrix specifying 
* translation, rotation & scale parameters
* IN:  tX, tY, tZ - translate components
*      rX, rY, rZ - rotate components
*      sX, sY, sZ - scale components
* OUT: pMat - pointer to result matrix
* RET: FALSE - one of the scale components is ZERO
*              (pMat is made ZERO)
*      TRUE  - all sX, sY, sZ components are non-ZERO
****************************************************/
BOOL m3dMATR::MakeTRS (float tX, float tY, float tZ, float rX, float rY, float rZ, float sX, float sY, float sZ)
{
   Identity();
   Translate(tX, tY, tZ, M3D_PRE);
   if (!m3dIsZero(rZ)) {
      RotateZ(rZ, M3D_PRE);
   }
   if (!m3dIsZero(rY)) {
      RotateY(rY, M3D_PRE);
   }
   if (!m3dIsZero(rX)) {
      RotateX(rX, M3D_PRE);
   }
   Scale(sX, sY, sZ, M3D_PRE);

   if (m3dIsZero_A(sX, 0.005f) || 
       m3dIsZero_A(sY, 0.005f) || 
       m3dIsZero_A(sZ, 0.005f)) {
      return FALSE;
   }
   return TRUE;
}

/***************************************************************
* m3dMATR::Orthonormalize()
*
***************************************************************/
void m3dMATR::Orthonormalize (void)
{
   m3dV x, y, z, org;

   GetAxisX(&x);
   GetAxisY(&y);
   GetAxisZ(&z);
   GetOrigin(&org);

   m3dNormalize(&x);
   m3dCrossProduct(&x, &y, &z); m3dNormalize(&z);
   m3dCrossProduct(&z, &x, &y); m3dNormalize(&y);

   MakeLCS2WCS(&org, &x, &y, &z);

   return;
}

/****************************************************
 * m3dMATR::MakeProjTri ()
 *
 * Calculate transform matrix for triangle
 * Parameters: 3 vertices of triangle
 * Return:     matrix
 ****************************************************/
void m3dMATR::MakeProjTri (m3dV *v1, m3dV *v2, m3dV *v3)
{
   m3dPOLY tmpPoly;
   m3dV    polyAxis1, polyAxis2, polyNorm;
#ifdef _DEBUG
   m3dV    vTmp;
#endif

   // Project polygon to self 2D plane
   m3dPolyNVert(&tmpPoly) = 3;
   *m3dPolyVertex(&tmpPoly, 0) = *v1;
   *m3dPolyVertex(&tmpPoly, 1) = *v2;
   *m3dPolyVertex(&tmpPoly, 2) = *v3;
   m3dGetPolyNormal(&tmpPoly, &polyNorm);

   // Create 2D CS
   m3dSubtractVector(v2, v1, &polyAxis1);
   m3dNormalize(&polyAxis1);
   m3dCrossProduct(&polyAxis1, &polyNorm, &polyAxis2);
   m3dNormalize(&polyAxis2);

   // Create transformation matrix
   Identity();
   ElementRC(0, 0) = polyAxis1.x;
   ElementRC(0, 1) = polyNorm.x;
   ElementRC(0, 2) = polyAxis2.x;
   ElementRC(1, 0) = polyAxis1.y;
   ElementRC(1, 1) = polyNorm.y;
   ElementRC(1, 2) = polyAxis2.y;
   ElementRC(2, 0) = polyAxis1.z;
   ElementRC(2, 1) = polyNorm.z;
   ElementRC(2, 2) = polyAxis2.z;
   Translate(-v1->x, -v1->y, -v1->z, M3D_PRE);

#ifdef _DEBUG
   TransformPoint(v1, &vTmp);
   ASSERT(m3dIsZero_A(vTmp.x, 1000 * M3D_EPSILON));
   ASSERT(m3dIsZero_A(vTmp.y, 1000 * M3D_EPSILON));
   ASSERT(m3dIsZero_A(vTmp.z, 1000 * M3D_EPSILON));
   TransformPoint(v2, &vTmp);
   ASSERT(m3dIsZero_A(vTmp.y, 1000 * M3D_EPSILON));
   TransformPoint(v3, &vTmp);
   ASSERT(m3dIsZero_A(vTmp.y, 1000 * M3D_EPSILON));
#endif
   return;
}


/********************************************************
* m3dMATR::MakePolyPoly ()
* IN:   pPolyFrom
*       pPolyTo    - two polygons
* OUT:  pMat - matrix of the affine transformation
*       transforming 3 first vertices of pPolyFrom into 
*       3 first vertices of pPolyTo
* RET:  FALSE if error occures (pPolyFrom or pPolyTo is degenerate)
*
*       The 4th vertices of polygons are always ignored
* NOTE: RESHENIJE NEODNOZNACHNO !!!
*********************************************************/
BOOL m3dMATR::MakePolyPoly (m3dPOLY *pPolyFrom, m3dPOLY *pPolyTo)
{
   BOOL    rc;
   m3dMATR matrFromToXZ, matrToToXZ, matrXZToTo, matr3D;
   m3dPOLY polyFromProj, polyToProj;
   m2dPOLY polyFrom2D,   polyTo2D;
   float   det;
   m2dMATR matr2D;
#ifdef _DEBUG
   int     i;
   m3dPOLY polyTmp;
#endif // _DEBUG

   matrFromToXZ.MakeProjTri(&pPolyFrom->vert[0], &pPolyFrom->vert[1], &pPolyFrom->vert[2]);
   matrToToXZ.MakeProjTri  (&pPolyTo->vert[0],   &pPolyTo->vert[1],   &pPolyTo->vert[2]);
   matrFromToXZ.TransformPoly(pPolyFrom, &polyFromProj);
   matrToToXZ.TransformPoly  (pPolyTo  , &polyToProj);

#ifdef _DEBUG
   for (i=0; i<3; i++) {
      ASSERT(m3dIsZero_A(m3dPolyVertexY(&polyFromProj,i), 0.001f));
      ASSERT(m3dIsZero_A(m3dPolyVertexY(&polyToProj  ,i), 0.001f));
   }
#endif

   m3xzPoly3DToPoly2D(&polyFromProj, &polyFrom2D);
   m3xzPoly3DToPoly2D(&polyToProj  , &polyTo2D  );

   rc = matr2D.MakePolyPoly(&polyFrom2D, &polyTo2D);
   if (!rc) {
      return FALSE;
   }
   // convert 2D matrix to 3D
   matr3D.Identity();
   matr3D.ElementRC(0, 0) = matr2D.ElementRC(0, 0);
   matr3D.ElementRC(0, 2) = matr2D.ElementRC(0, 1);
   matr3D.ElementRC(2, 0) = matr2D.ElementRC(1, 0);
   matr3D.ElementRC(2, 2) = matr2D.ElementRC(1, 1);
   matr3D.ElementRC(3, 0) = matr2D.ElementRC(2, 0);
   matr3D.ElementRC(3, 2) = matr2D.ElementRC(2, 1);

   // scale 3rd dimension to improve matrix condition value
   det = matr3D.CalcDet();
   ASSERT(det > 0);
   matr3D.Scale(1.f, m3dSqrt(m3dFAbs(det)), 1.f, M3D_POST);

#ifdef _DEBUG
   matr3D.TransformPoly(&polyFromProj, &polyTmp);
   for (i=0; i<3; i++) {
      ASSERT(m3dIsZero_A(m3dDist(m3dPolyVertex(&polyTmp,i), m3dPolyVertex(&polyToProj,i)),0.001f));
   }
#endif

   rc = matrToToXZ.Invert(&matrXZToTo);
   if (!rc) {
      return FALSE;
   }
   *this = matrFromToXZ;
   Transform(&matr3D, M3D_POST);

#ifdef _DEBUG
   TransformPoly(pPolyFrom, &polyTmp);
   for (i=0; i<3; i++) {
      ASSERT(m3dIsZero_A(m3dDist_2(m3dPolyVertex(&polyTmp,i), m3dPolyVertex(&polyToProj,i)), 0.0001f));
   }
#endif
   
   Transform(&matrXZToTo, M3D_POST);

#ifdef _DEBUG
   TransformPoly(pPolyFrom, &polyTmp);
   for (i=0; i<3; i++) {
      ASSERT(m3dIsZero_A(m3dDist_2(m3dPolyVertex(&polyTmp,i), m3dPolyVertex(pPolyTo,i)),0.0001f));
   }
#endif
   return TRUE;
}

/********************************************************
* m3dMATR::MakeTriTri ()
*
*********************************************************/
BOOL m3dMATR::MakeTriTri (m3dTRI *pTriFrom, m3dTRI *pTriTo)
{
   m3dPOLY polyFrom, polyTo;
   polyFrom = *pTriFrom;
   polyTo   = *pTriTo;
   return MakePolyPoly(&polyFrom, &polyTo);
}


//   D3DXMATRIX* WINAPI D3DXMatrixLookAtLH( D3DXMATRIX *pOut, CONST D3DXVECTOR3 *pEye, CONST D3DXVECTOR3 *pAt,CONST D3DXVECTOR3 *pUp );
//   D3DXMATRIX* WINAPI D3DXMatrixPerspectiveLH( D3DXMATRIX *pOut, FLOAT w, FLOAT h, FLOAT zn, FLOAT zf );
//   D3DXMATRIX* WINAPI D3DXMatrixPerspectiveFovLH( D3DXMATRIX *pOut, FLOAT fovy, FLOAT Aspect, FLOAT zn, FLOAT zf );
//   D3DXMATRIX* WINAPI D3DXMatrixPerspectiveOffCenterLH( D3DXMATRIX *pOut, FLOAT l, FLOAT r, FLOAT b, FLOAT t, FLOAT zn,FLOAT zf );
//   D3DXMATRIX* WINAPI D3DXMatrixOrthoLH ( D3DXMATRIX *pOut, FLOAT w, FLOAT h, FLOAT zn, FLOAT zf );
//   D3DXMATRIX* WINAPI D3DXMatrixOrthoOffCenterLH ( D3DXMATRIX *pOut, FLOAT l, FLOAT r, FLOAT b, FLOAT t, FLOAT zn,FLOAT zf );

/**************************************************************
* m3dMATR::MakeLookAt ()
*    Build a lookat matrix. (left-handed)
* IN  : pEye - eye point
*       pAt  - camera look-at target
*       pUp  - current world's up, usually [0, 1, 0] 
**************************************************************/
void m3dMATR::MakeLookAt (const m3dV *pEye, const m3dV *pAt,const m3dV *pUp)
{
   m3dV   zaxis, xaxis, yaxis;

   m3dSubtractVector(pAt, pEye, &zaxis);
   m3dNormalize(&zaxis);

   m3dCrossProduct(pUp, &zaxis, &xaxis);
   m3dNormalize(&xaxis);

   m3dCrossProduct(&zaxis, &xaxis, &yaxis);

   m[0][0] = xaxis.x; m[0][1] = yaxis.x; m[0][2] = zaxis.x; m[0][3] = 0;
   m[1][0] = xaxis.y; m[1][1] = yaxis.y; m[1][2] = zaxis.y; m[1][3] = 0;
   m[2][0] = xaxis.z; m[2][1] = yaxis.z; m[2][2] = zaxis.z; m[2][3] = 0;
   m[3][0] = -m3dDotProduct(&xaxis, pEye); 
   m[3][1] = -m3dDotProduct(&yaxis, pEye);
   m[3][2] = -m3dDotProduct(&zaxis, pEye); 
   m[3][3] = 1;
   return;
}
/**************************************************************
* m3dMATR::MakeViewForwUp()
*    Build a view matrix. (left-handed)
* IN  : pEye - eye point
*       pForw  - camera forward direction (must be normalized)
*       pUp  - camera up (must be normalized)
**************************************************************/
void m3dMATR::MakeViewForwUp(const m3dV *pEye, const m3dV *pForw, const m3dV *pUp)
{
   m3dV   zaxis, xaxis, yaxis;

   zaxis = *pForw;
   m3dCrossProduct(pUp, &zaxis, &xaxis);
   yaxis = *pUp;

   m[0][0] = xaxis.x; m[0][1] = yaxis.x; m[0][2] = zaxis.x; m[0][3] = 0;
   m[1][0] = xaxis.y; m[1][1] = yaxis.y; m[1][2] = zaxis.y; m[1][3] = 0;
   m[2][0] = xaxis.z; m[2][1] = yaxis.z; m[2][2] = zaxis.z; m[2][3] = 0;
   m[3][0] = -m3dDotProduct(&xaxis, pEye); 
   m[3][1] = -m3dDotProduct(&yaxis, pEye);
   m[3][2] = -m3dDotProduct(&zaxis, pEye); 
   m[3][3] = 1;
   return;
}
/**************************************************************
* m3dMATR::MakePersp ()
*    Build a perspective projection matrix. (left-handed)
* IN  : w  - Width of the view volume at the near view-plane
*       h  - Height of the view volume at the near view-plane
*       zn - Z-value of the near view-plane
*       zf - Z-value of the far view-plane
**************************************************************/
void m3dMATR::MakePersp (float w, float h, float zn, float zf) 
{
   m[0][0] = 2*zn/w; m[0][1] = 0     ; m[0][2] = 0            ; m[0][3] = 0;
   m[1][0] = 0     ; m[1][1] = 2*zn/h; m[1][2] = 0            ; m[1][3] = 0;

#ifdef VID_USE_INVERTED_Z
   dsSwap(zn, zf);
#endif

   m[2][0] = 0     ; m[2][1] = 0     ; m[2][2] = zf/(zf-zn)   ; m[2][3] = 1;
   m[3][0] = 0     ; m[3][1] = 0     ; m[3][2] = zn*zf/(zn-zf); m[3][3] = 0;
   return;
}
/**************************************************************
* m3dMATR::MakePerspFOV ()
*    Build a perspective projection matrix. (left-handed)
* IN  : fovy   - Field of view in the y direction, in radians
*       Aspect - Aspect ratio, defined as view space width divided by height
*       zn - Z-value of the near view-plane
*       zf - Z-value of the far view-plane
**************************************************************/
void m3dMATR::MakePerspFOV (float fovy, float Aspect, float zn, float zf)
{
#ifdef VID_USE_INVERTED_Z
   dsSwap(zn, zf);
#endif
   float h; //is the view space height. 
   float w; //is the view space width
   
   h = 1.f/tanf(fovy/2.f);
   w = h / Aspect;

   m[0][0] = w; m[0][1] = 0; m[0][2] = 0             ; m[0][3] = 0;
   m[1][0] = 0; m[1][1] = h; m[1][2] = 0             ; m[1][3] = 0;
   m[2][0] = 0; m[2][1] = 0; m[2][2] = zf/(zf-zn)    ; m[2][3] = 1;
   m[3][0] = 0; m[3][1] = 0; m[3][2] = -zn*zf/(zf-zn); m[3][3] = 0;

   return;
}
/**************************************************************
* m3dMATR::MakePerspOffCenter ()
*    Build a perspective projection matrix. (left-handed)
* IN  : l  - Minimum x-value of the view volume
*       r  - Maximum x-value of the view volume
*       b  - Minimum y-value of the view volume
*       t  - Maximum y-value of the view volume
*       zn - Z-value of the near view-plane
*       zf - Z-value of the far view-plane
**************************************************************/
void m3dMATR::MakePerspOffCenter (float l, float r, float b, float t, float zn,float zf)
{
   m[0][0] = 2*zn/(r-l) ; m[0][1] = 0          ; m[0][2] = 0            ; m[0][3] = 0;
   m[1][0] = 0          ; m[1][1] = 2*zn/(t-b) ; m[1][2] = 0            ; m[1][3] = 0;

#ifdef VID_USE_INVERTED_Z
   dsSwap(zn, zf);
#endif

   m[2][0] = (l+r)/(l-r); m[2][1] = (t+b)/(b-t); m[2][2] = zf/(zf-zn)   ; m[2][3] = 1;
   m[3][0] = 0          ; m[3][1] = 0          ; m[3][2] = zn*zf/(zn-zf); m[3][3] = 0;

   return;
}
/**************************************************************
* m3dMATR::MakeOrtho ()
*    Build an ortho projection matrix. (left-handed)
* IN  : w  - Width of the view volume at the near view-plane
*       h  - Height of the view volume at the near view-plane
*       zn - Z-value of the near view-plane
*       zf - Z-value of the far view-plane
**************************************************************/
void m3dMATR::MakeOrtho (float w, float h, float zn, float zf) 
{
   m[0][0] = 2/w; m[0][1] = 0  ; m[0][2] = 0         ; m[0][3] = 0;
   m[1][0] = 0  ; m[1][1] = 2/h; m[1][2] = 0         ; m[1][3] = 0;
   m[2][0] = 0  ; m[2][1] = 0  ; m[2][2] = 1/(zf-zn) ; m[2][3] = 0;
   m[3][0] = 0  ; m[3][1] = 0  ; m[3][2] = zn/(zn-zf); m[3][3] = 1;
   return;
}
/**************************************************************
* m3dMATR::MakeOrthoOffCenter ()
*    Build an ortho projection matrix. (left-handed)
* IN  : l  - Minimum x-value of the view volume
*       r  - Maximum x-value of the view volume
*       b  - Minimum y-value of the view volume
*       t  - Maximum y-value of the view volume
*       zn - Z-value of the near view-plane
*       zf - Z-value of the far view-plane
**************************************************************/
void m3dMATR::MakeOrthoOffCenter (float l, float r, float b, float t, float zn,float zf)
{
#ifdef VID_USE_INVERTED_Z
   dsSwap(zn, zf);
#endif
   m[0][0] = 2/(r-l)    ; m[0][1] = 0          ; m[0][2] = 0         ; m[0][3] = 0;
   m[1][0] = 0          ; m[1][1] = 2/(t-b)    ; m[1][2] = 0         ; m[1][3] = 0;
   m[2][0] = 0          ; m[2][1] = 0          ; m[2][2] = 1/(zf-zn) ; m[2][3] = 0;
   m[3][0] = (l+r)/(l-r); m[3][1] = (t+b)/(b-t); m[3][2] = zn/(zn-zf); m[3][3] = 1;
   return;
}

/**************************************************************
* m3dMATR::CalcNorm()
* IN : pMatr - matrix
* OUT: none
* RET: norm of pMatr
* NOTE: The norm used is a maximum of sums of modules of 
*       matrix row elements
**************************************************************/
float m3dMATR::CalcNorm (void) const
{
   int   i,j;
   float max_sum = 0.f;
   float sum;

   for (i=0; i<3; i++) {
      sum = 0.f;
      for (j=0; j<3; j++) {
         sum += m3dFAbs(ElementRC(i,j));
      }
      if (sum > max_sum) {
         max_sum = sum;
      }
   }
   return max_sum;
}

/**************************************************************
* m3dMATR::CalcCondValue()
* Calculate matrix condition value
* (\rus CHISLO OBUSLOVLENNOSTI)
* IN  : pMatr - matrix
* OUT : none
* RET : condition value = norm(pMatr) * norm(pMatr^{-1})
* NOTE: The norm used in 'm3dMatrNorm()' is NOT Euclidian !
*       The condition value depends on the norm function, see 'm3dMatrNorm()'
**************************************************************/
float m3dMATR::CalcCondValue (void) const
{
   m3dMATR matr_inv;
   float   cond;

   if (!Invert(&matr_inv)) {
      return M3D_INFINITY;
   }
   cond = CalcNorm() * matr_inv.CalcNorm();
//
// THEOREM. For any matrix 'pMatr' and for any matrix norm:
//          norm(pMatr)*norm(pMatr_inv) >= 1
// PROOF.   It follows from the property of the matrix norm that:
//          norm(A)*norm(B) >= norm (A*B).
//          Therefore norm(pMatr)*norm(pMatr_inv) >= norm (Identity) = 1
//
   ASSERT(cond >= 1.f - 10*M3D_EPSILON);
   return cond;
}

static const float _M3D_MATR_CHECK_ORTO_DEGREE_ACC = 2.f; 
/*************************************************************
* m3dMATR::Check()
*
* IN: pMat - matrix to check
*
* Check if matrix is an orthogonal transformation matrix
* NOTE: normalization is not checked
*************************************************************/
BOOL m3dMATR::Check (void) const 
{
   int   r, c, i;
   float sum;

   /*
#ifndef _DEBUG
   return;
#endif
   */

   for (i = 0; i < 16; i++) {
      if (!_m3dCheckValid(Element(i))) {
         return FALSE;
      }
   }

   // check that last column is [0,0,0,1]
   for (c = 0; c < 3; c++)  {
      ASSERT(m3dIsZero(ElementRC(c,3)));
   }
   ASSERT(m3dIsZero(ElementRC(3,3) - 1.f));

   // check axis are orto 
#if !defined(_RETAIL) && !defined(_PROFILE)
   {
      float sx, sy, sz;
      GetScale(&sx, &sy, &sz);
      if (m3dIsZero_A(sx-sy,0.0001f) && m3dIsZero_A(sy-sz,0.0001f)) {
         m3dV v1 = *GetAxisX();
         m3dV v2 = *GetAxisY();
         m3dV v3 = *GetAxisZ();
         _m3dNormalize(&v1);
         _m3dNormalize(&v2);
         _m3dNormalize(&v3);
         float ang12 = 90.f - m3dAngleVector(&v1, &v2);
         ASSERT(m3dIsZero_A(ang12,_M3D_MATR_CHECK_ORTO_DEGREE_ACC));
         float ang13 = 90.f - m3dAngleVector(&v1, &v3);
         ASSERT(m3dIsZero_A(ang13,_M3D_MATR_CHECK_ORTO_DEGREE_ACC));
         float ang23 = 90.f - m3dAngleVector(&v2, &v3);
         ASSERT(m3dIsZero_A(ang23,_M3D_MATR_CHECK_ORTO_DEGREE_ACC));
      } else {
         // don't check this case
      }
   }
#endif

   // check that matrix rows are not zero
   for (r = 0; r < 3; r++)  {
      sum = 0.f;
      for (c = 0; c < 3; c++)  {
         sum += m3dPow2(ElementRC(r,c));
      }
      ASSERT(sum > 0.00001f*M3D_EPSILON);
   }

   // check that matrix columns are not zero
   for (c = 0; c < 3; c++)  {
      sum = 0.f;
      for (r = 0; r < 3; r++)  {
         sum += m3dPow2(ElementRC(r,c));
      }
      ASSERT(sum > 0.00001f*M3D_EPSILON);
   }

   //float dot = m3dDotProduct(GetAxisX(), GetAxisY());
   //ASSERT(m3dIsZero_A(dot,0.02f));
   //dot = m3dDotProduct(GetAxisX(), GetAxisZ());
   //ASSERT(m3dIsZero_A(dot,0.02f));
   //dot = m3dDotProduct(GetAxisY(), GetAxisZ());
   //ASSERT(m3dIsZero_A(dot,0.02f));
   return TRUE;
}

/*************************************************************
* m3dMATR::CheckOrtho()
*
* IN: pMat - matrix to check
*
* Check if matrix is orthonormal transformation matrix
*************************************************************/
void m3dMATR::CheckOrtho (void) const
{
   int   r, c;
   float sum;

   // check that last column is [0,0,0,1]
   for (c = 0; c < 3; c++)  {
      ASSERT(m3dIsZero(ElementRC(c, 3)));
   }
   ASSERT(m3dIsZero(ElementRC(3, 3) - 1.f));

   // check that matrix rows are normalized
   for (r = 0; r < 3; r++)  {
      sum = 0.f;
      for (c = 0; c < 3; c++)  {
         sum += m3dPow2(ElementRC(r, c));
      }
      ASSERT(m3dIsZero_A(sum-1.f,_M3D_MATR_CHECK_ACC));
   }

   // check that matrix columns are normalized
   for (c = 0; c < 3; c++)  {
      sum = 0.f;
      for (r = 0; r < 3; r++)  {
         sum += m3dPow2(ElementRC(r,c));
      }
      ASSERT(m3dIsZero_A(sum-1.f,_M3D_MATR_CHECK_ACC));
   }

   // check that matrix is orthogonal
   Check();

   return;
}

/*************************************************************
* m3dMATR::Transpose()
* 
*************************************************************/
void m3dMATR::Transpose (void)
{
#if !defined M3D_SUPPRESS_OPTIMIZATION
   vmxMATR m = vmxMatrLoadUA((float*)this->elements);
   m = vmxMatrTranspose(m);
   vmxMatrStoreUA(m, this->elements);
#else
   m3dSwap(ElementRC(0, 1), ElementRC(1, 0));
   m3dSwap(ElementRC(0, 2), ElementRC(2, 0));
   m3dSwap(ElementRC(0, 3), ElementRC(3, 0));
   m3dSwap(ElementRC(1, 2), ElementRC(2, 1));
   m3dSwap(ElementRC(1, 3), ElementRC(3, 1));
   m3dSwap(ElementRC(2, 3), ElementRC(3, 2));
#endif
/*
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   vec_float4 a = vec_load_unaligned(elements, 0);
   vec_float4 b = vec_load_unaligned(elements, 16);
   vec_float4 c = vec_load_unaligned(elements, 32);
   vec_float4 d = vec_load_unaligned(elements, 48);

   vec_float4 tmp0 = vec_mergeh(a, c);
   vec_float4 tmp1 = vec_mergeh(b, d);
   vec_float4 tmp2 = vec_mergel(a, c);
   vec_float4 tmp3 = vec_mergel(b, d);

   vec_float4 r0 = vec_mergeh(tmp0, tmp1);
   vec_float4 r1 = vec_mergel(tmp0, tmp1);
   vec_float4 r2 = vec_mergeh(tmp2, tmp3);
   vec_float4 r3 = vec_mergel(tmp2, tmp3);

   vec_store_unaligned(r0, elements, 0);
   vec_store_unaligned(r1, elements, 16);
   vec_store_unaligned(r2, elements, 32);
   vec_store_unaligned(r3, elements, 48);
#elif defined _AP_PC
   __m128 mm0=_mm_loadu_ps(&_11);
   __m128 mm1=_mm_loadu_ps(&_21);
   __m128 mm2=_mm_loadu_ps(&_31);
   __m128 mm3=_mm_loadu_ps(&_41);

   _MM_TRANSPOSE4_PS(mm0, mm1, mm2, mm3);

   _mm_storeu_ps(&_11, mm0);
   _mm_storeu_ps(&_21, mm1);
   _mm_storeu_ps(&_31, mm2);
   _mm_storeu_ps(&_41, mm3);
#else
   m3dSwap(ElementRC(0, 1), ElementRC(1, 0));
   m3dSwap(ElementRC(0, 2), ElementRC(2, 0));
   m3dSwap(ElementRC(0, 3), ElementRC(3, 0));
   m3dSwap(ElementRC(1, 2), ElementRC(2, 1));
   m3dSwap(ElementRC(1, 3), ElementRC(3, 1));
   m3dSwap(ElementRC(2, 3), ElementRC(3, 2));
#endif
*/
   return;
}



/*************************************************************
* m3dMATR::Invert()
*
* Invert 3D matrix
* IN : srcMatr  - matric to be inverted
* OUT: destMatr - inverted matrix
* RET: FALSE if matrix can't be inverted (zero determinant)
* NOTE: Matrix must be a valid 3D transformation matrix,
*       last column must be [0 0 0 ?]
*************************************************************/
/*BOOL m3dMATR::Invert(m3dMATR *destMatr) const
{
   float  det, oodet;
   short  i, j, row1 = 0, row2 = 0, row3 = 0, col1 = 0, col2 = 0, col3 = 0; 
#ifndef _RELEASE
   m3dMATR   testMatr, matrIdentity;
#endif

   if (m3dIsZero(ElementRC(3, 3))) {
      // for inverting projection matrices
      ASSERT(!m3dIsZero(ElementRC(2, 3)));
      det = - ElementRC(2, 3) * _CalcMatrDet(0, 1, 3, 0, 1, 2);
   } else {
      det = CalcDet();
   }
   if (m3dIsZero_A(det,m3dPow2(M3D_EPSILON))) {
      // normally, all matrices must be non-degenerate
      // ASSERT(FALSE);
      return FALSE;
   }
   oodet = 1.f / det;

   // Calculate all destination matrix elements
   for (i=0; i<4; i++) {
      for (j=0; j<4; j++) {
         switch (i) {
            case 0: row1 = 1; row2 = 2; row3 = 3; break;
            case 1: row1 = 0; row2 = 2; row3 = 3; break;
            case 2: row1 = 0; row2 = 1; row3 = 3; break;
            case 3: row1 = 0; row2 = 1; row3 = 2; break;
         }
         switch (j) {
            case 0: col1 = 1; col2 = 2; col3 = 3; break;
            case 1: col1 = 0; col2 = 2; col3 = 3; break;
            case 2: col1 = 0; col2 = 1; col3 = 3; break;
            case 3: col1 = 0; col2 = 1; col3 = 2; break;
         }
         if ((i+j) % 2) {
            destMatr->ElementRC(j, i) = -_CalcMatrDet(row1, row2, row3, col1, col2, col3) * oodet;
         } else {
            destMatr->ElementRC(j, i) =  _CalcMatrDet(row1, row2, row3, col1, col2, col3) * oodet;
         }
      }
   }

//#ifndef _RELEASE
//   m3dMultiplyMatr(this, destMatr, &testMatr);
//   matrIdentity.Identity();
//   ASSERT(m3dIsEqualMatr_A(&testMatr, &matrIdentity, 0.005f, 0.005f));
//#endif

   return TRUE;
}*/

#define MATRIX_INVERSE_EPSILON   1e-14

/*************************************************************
* m3dMATR::Invert()
*
* Invert 4x4 matrix
* IN : srcMatr  - matric to be inverted
* OUT: destMatr - inverted matrix
* RET: FALSE if matrix can't be inverted (zero determinant)
*
* Source code from DOOM3_SDK_2 matrix.cpp
*************************************************************/
BOOL m3dMATR::Invert(m3dMATR *destMatr) const
{
   // 84+4+16 = 104 multiplications
   //          1 division
   double det, invDet;

   // 2x2 sub-determinants required to calculate 4x4 determinant
   float det2_01_01 = m[0][0] * m[1][1] - m[0][1] * m[1][0];
   float det2_01_02 = m[0][0] * m[1][2] - m[0][2] * m[1][0];
   float det2_01_03 = m[0][0] * m[1][3] - m[0][3] * m[1][0];
   float det2_01_12 = m[0][1] * m[1][2] - m[0][2] * m[1][1];
   float det2_01_13 = m[0][1] * m[1][3] - m[0][3] * m[1][1];
   float det2_01_23 = m[0][2] * m[1][3] - m[0][3] * m[1][2];

   // 3x3 sub-determinants required to calculate 4x4 determinant
   float det3_201_012 = m[2][0] * det2_01_12 - m[2][1] * det2_01_02 + m[2][2] * det2_01_01;
   float det3_201_013 = m[2][0] * det2_01_13 - m[2][1] * det2_01_03 + m[2][3] * det2_01_01;
   float det3_201_023 = m[2][0] * det2_01_23 - m[2][2] * det2_01_03 + m[2][3] * det2_01_02;
   float det3_201_123 = m[2][1] * det2_01_23 - m[2][2] * det2_01_13 + m[2][3] * det2_01_12;

   det = ( - det3_201_123 * m[3][0] + det3_201_023 * m[3][1] - det3_201_013 * m[3][2] + det3_201_012 * m[3][3] );

   if ( fabs( det ) < MATRIX_INVERSE_EPSILON ) {
      return FALSE;
   }

   invDet = 1.0 / det;

   // remaining 2x2 sub-determinants
   float det2_03_01 = m[0][0] * m[3][1] - m[0][1] * m[3][0];
   float det2_03_02 = m[0][0] * m[3][2] - m[0][2] * m[3][0];
   float det2_03_03 = m[0][0] * m[3][3] - m[0][3] * m[3][0];
   float det2_03_12 = m[0][1] * m[3][2] - m[0][2] * m[3][1];
   float det2_03_13 = m[0][1] * m[3][3] - m[0][3] * m[3][1];
   float det2_03_23 = m[0][2] * m[3][3] - m[0][3] * m[3][2];

   float det2_13_01 = m[1][0] * m[3][1] - m[1][1] * m[3][0];
   float det2_13_02 = m[1][0] * m[3][2] - m[1][2] * m[3][0];
   float det2_13_03 = m[1][0] * m[3][3] - m[1][3] * m[3][0];
   float det2_13_12 = m[1][1] * m[3][2] - m[1][2] * m[3][1];
   float det2_13_13 = m[1][1] * m[3][3] - m[1][3] * m[3][1];
   float det2_13_23 = m[1][2] * m[3][3] - m[1][3] * m[3][2];

   // remaining 3x3 sub-determinants
   float det3_203_012 = m[2][0] * det2_03_12 - m[2][1] * det2_03_02 + m[2][2] * det2_03_01;
   float det3_203_013 = m[2][0] * det2_03_13 - m[2][1] * det2_03_03 + m[2][3] * det2_03_01;
   float det3_203_023 = m[2][0] * det2_03_23 - m[2][2] * det2_03_03 + m[2][3] * det2_03_02;
   float det3_203_123 = m[2][1] * det2_03_23 - m[2][2] * det2_03_13 + m[2][3] * det2_03_12;

   float det3_213_012 = m[2][0] * det2_13_12 - m[2][1] * det2_13_02 + m[2][2] * det2_13_01;
   float det3_213_013 = m[2][0] * det2_13_13 - m[2][1] * det2_13_03 + m[2][3] * det2_13_01;
   float det3_213_023 = m[2][0] * det2_13_23 - m[2][2] * det2_13_03 + m[2][3] * det2_13_02;
   float det3_213_123 = m[2][1] * det2_13_23 - m[2][2] * det2_13_13 + m[2][3] * det2_13_12;

   float det3_301_012 = m[3][0] * det2_01_12 - m[3][1] * det2_01_02 + m[3][2] * det2_01_01;
   float det3_301_013 = m[3][0] * det2_01_13 - m[3][1] * det2_01_03 + m[3][3] * det2_01_01;
   float det3_301_023 = m[3][0] * det2_01_23 - m[3][2] * det2_01_03 + m[3][3] * det2_01_02;
   float det3_301_123 = m[3][1] * det2_01_23 - m[3][2] * det2_01_13 + m[3][3] * det2_01_12;

   destMatr->m[0][0] = float(- det3_213_123 * invDet);
   destMatr->m[1][0] = float(+ det3_213_023 * invDet);
   destMatr->m[2][0] = float(- det3_213_013 * invDet);
   destMatr->m[3][0] = float(+ det3_213_012 * invDet);

   destMatr->m[0][1] = float(+ det3_203_123 * invDet);
   destMatr->m[1][1] = float(- det3_203_023 * invDet);
   destMatr->m[2][1] = float(+ det3_203_013 * invDet);
   destMatr->m[3][1] = float(- det3_203_012 * invDet);

   destMatr->m[0][2] = float(+ det3_301_123 * invDet);
   destMatr->m[1][2] = float(- det3_301_023 * invDet);
   destMatr->m[2][2] = float(+ det3_301_013 * invDet);
   destMatr->m[3][2] = float(- det3_301_012 * invDet);

   destMatr->m[0][3] = float(- det3_201_123 * invDet);
   destMatr->m[1][3] = float(+ det3_201_023 * invDet);
   destMatr->m[2][3] = float(- det3_201_013 * invDet);
   destMatr->m[3][3] = float(+ det3_201_012 * invDet);

#ifdef _DEBUG
   for (int i = 0; i < 16; i++) {
      if (!_m3dCheckValid(*(&destMatr->m[0][0] + i))) {
         return FALSE;
      }
   }
#endif
   return TRUE;
}



/*************************************************************
* m3dMATR::CalcDet ()
*
* Compute matric determinant
* IN  : pMatr - matrix
* RET : matrix determinant
* NOTE: matrix must be a valid 3D transformation matrix
*************************************************************/
float m3dMATR::CalcDet (void) const
{
   return ElementRC(3, 3) * _CalcMatrDet(0, 1, 2, 0, 1, 2);
}

/*************************************************************
* m3dMATR::RemoveScale()
* 
*************************************************************/
void m3dMATR::RemoveScale (void)
{
   float    isx, isy, isz;

   GetInvScale(&isx, &isy, &isz);
   Scale(isx, isy, isz, M3D_PRE);

   return;
}

/*************************************************************
* m3dMATR::TransformPointList()
*
* Transform array of points
*************************************************************/
void __CDECL m3dMATR::TransformPointList (int nPoint, void *srcPtr, int srcStride, void *dstPtr, int dstStride) const
{
   ASSERT(srcStride >= sizeof(m3dV));
   ASSERT(dstStride >= sizeof(m3dV));

#if defined(_AP_XENON) && !defined(_NO_XMM)
   XMMATRIX m(elements);
   XMVector3TransformCoordStream((XMFLOAT3*)dstPtr, dstStride, (XMFLOAT3*)srcPtr, srcStride, nPoint, m);
   return;
#elif defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   // Load matrix
   vec_float4 ma, mb, mc, md;
	ma = vec_load_unaligned(elements, 0);
	mb = vec_load_unaligned(elements, 16);
	mc = vec_load_unaligned(elements, 32);
	md = vec_load_unaligned(elements, 48);

   unsigned char *src = (unsigned char *)srcPtr;
   unsigned char *dst = (unsigned char *)dstPtr;

   for (int i = 0; i < nPoint; ++i)
   {
      vec_float4 v = vec_load_unaligned((float*)src);

      vec_float4 x = vec_splat(v, 0);
      vec_float4 y = vec_splat(v, 1);
      vec_float4 z = vec_splat(v, 2);

      vec_float4 r;
      r = vec_madd(z, mc, md);
      r = vec_madd(y, mb, r);
      r = vec_madd(x, ma, r);

      vec_float4 iw = vec_splat(r, 3);
      iw = vec_re(iw);

      r = vec_madd(r, iw, (vec_float4)(0.0f));
      vec_store3_unaligned(r, (float*)dst);

      src += srcStride;
      dst += dstStride;
   }
   return;
#elif defined M3D_USE_SSE
   __asm {
      mov         ecx, nPoint
      mov         eax, this
      test        ecx, ecx
      je          _end
      movlps      xmm4, qword ptr [eax]
      mov         esi, srcPtr
      movhps      xmm4, qword ptr [eax+8]
      movlps      xmm5, qword ptr [eax+10h]
      mov         edi, dstPtr
      movhps      xmm5, qword ptr [eax+18h]
      movlps      xmm6, qword ptr [eax+20h]
      mov         ebx, srcStride
      movhps      xmm6, qword ptr [eax+28h]
      movlps      xmm7, qword ptr [eax+30h]
      mov         edx, dstStride
      movhps      xmm7, qword ptr [eax+38h]
_loop:
      prefetchnta [esi + 32]
      movss       xmm0, dword ptr [esi]
      movss       xmm1, dword ptr [esi+4]
      shufps      xmm0, xmm0, 0
      movss       xmm2, dword ptr [esi+8]
      add         esi, ebx
      mulps       xmm0, xmm4
      shufps      xmm1, xmm1, 0
      shufps      xmm2, xmm2, 0
      mulps       xmm1, xmm5
      addps       xmm0, xmm7
      mulps       xmm2, xmm6
      addps       xmm0, xmm1
      addps       xmm0, xmm2
      movlps      qword ptr [edi], xmm0
      shufps      xmm0, xmm0, 2
      movss       dword ptr [edi+8], xmm0
      add         edi, edx
      dec         ecx
      jne         _loop
_end:
   }
#else
   if (srcPtr == dstPtr) {
      for (int i = 0; i < nPoint; i++) {
         TransformPoint((m3dV*)srcPtr);
         srcPtr = ((BYTE*)srcPtr) + srcStride;
      }
   } else {
      for (int i = 0; i < nPoint; i++) {
         TransformPoint((m3dV*)srcPtr, (m3dV*)dstPtr);
         srcPtr = ((BYTE*)srcPtr) + srcStride;
         dstPtr = ((BYTE*)dstPtr) + dstStride;
      }
   }
#endif
}

/*************************************************************
* m3dMATR::TransformVectorList()
*
* Transform array of vectors
* IN: srcStride, dstStride - size of structure in which
*     vertices to be transformed are stored; by default
*     srcPtr/dstPtr are simply vert lists => 
*     srcStride/dstSride = sizeof(m3dV)
*************************************************************/
void __CDECL m3dMATR::TransformVectorList (int nVector, void *srcPtr, int srcStride, void *dstPtr, int dstStride) const
{
   ASSERT(srcStride >= sizeof(m3dV));
   ASSERT(dstStride >= sizeof(m3dV));

#if defined(_AP_XENON) && !defined(_NO_XMM)
   XMMATRIX m(elements);
   XMVector3TransformNormalStream((XMFLOAT3*)dstPtr, dstStride, (XMFLOAT3*)srcPtr, srcStride, nVector, m);
   return;
#elif defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   // Load matrix
   vec_float4 ma, mb, mc;
	ma = vec_load_unaligned(elements, 0);
	mb = vec_load_unaligned(elements, 16);
	mc = vec_load_unaligned(elements, 32);

   unsigned char *src = (unsigned char *)srcPtr;
   unsigned char *dst = (unsigned char *)dstPtr;

   for (int i = 0; i < nVector; ++i)
   {
      vec_float4 v = vec_load_unaligned((float*)src);

      vec_float4 x = vec_splat(v, 0);
      vec_float4 y = vec_splat(v, 1);
      vec_float4 z = vec_splat(v, 2);

      vec_float4 r;
      r = vec_madd(z, mc, (vec_float4)(0.0f));
      r = vec_madd(y, mb, r);
      r = vec_madd(x, ma, r);

      vec_store3_unaligned(r, (float*)dst);

      src += srcStride;
      dst += dstStride;
   }
   return;
#elif defined M3D_USE_SSE
   __asm {
      mov         ecx, nVector
      test        ecx, ecx
      je          _end
      mov         eax, this
      movlps      xmm4, qword ptr [eax]
      mov         esi, srcPtr
      movhps      xmm4, qword ptr [eax+8]
      movlps      xmm5, qword ptr [eax+10h]
      mov         edi, dstPtr
      movhps      xmm5, qword ptr [eax+18h]
      movlps      xmm6, qword ptr [eax+20h]
      mov         ebx, srcStride
      movhps      xmm6, qword ptr [eax+28h]
      //movlps      xmm7, qword ptr [eax+30h]
      mov         edx, dstStride
      //movhps      xmm7, qword ptr [eax+38h]
_loop:
      movss       xmm0, dword ptr [esi]
      movss       xmm1, dword ptr [esi+4]
      shufps      xmm0, xmm0, 0
      movss       xmm2, dword ptr [esi+8]
      add         esi, ebx
      mulps       xmm0, xmm4
      shufps      xmm1, xmm1, 0
      shufps      xmm2, xmm2, 0
      mulps       xmm1, xmm5
      //addps       xmm0, xmm7
      mulps       xmm2, xmm6
      addps       xmm0, xmm1
      addps       xmm0, xmm2
      movlps      qword ptr [edi], xmm0
      shufps      xmm0, xmm0, 2
      movss       dword ptr [edi+8], xmm0
      add         edi, edx
      dec         ecx
      jne         _loop
_end:
   }

#else
   if (srcPtr==dstPtr) {
      for (int i = 0; i < nVector; i++) {
         TransformVector((m3dV*)srcPtr);
         srcPtr = ((BYTE*)srcPtr) + srcStride;
      }
   } else {
      for (int i = 0; i < nVector; i++) {
         TransformVector((m3dV*)srcPtr, (m3dV*)dstPtr);
         srcPtr = ((BYTE*)srcPtr) + srcStride;
         dstPtr = ((BYTE*)dstPtr) + dstStride;
      }
   }
#endif
}

/*************************************************************
* Static functions
*************************************************************/
/*************************************************************
* m3dMATR::_CalcMatrDet()
*
* Calculate determinant of 3*3 matrix, a sub-matrix of
* a 4x4 matrix
* IN:  pMatr       - matrix 4*4
*      row?, col?  - rows & columns
* RET: 3x3 matrix determinant
*************************************************************/
float m3dMATR::_CalcMatrDet (short row1, short row2, short row3, short col1, short col2, short col3) const
{
   float det;

   det  = ElementRC(row1, col1)*ElementRC(row2, col2)*ElementRC(row3, col3);
   det += ElementRC(row1, col3)*ElementRC(row2, col1)*ElementRC(row3, col2);
   det += ElementRC(row1, col2)*ElementRC(row2, col3)*ElementRC(row3, col1);
   det -= ElementRC(row1, col3)*ElementRC(row2, col2)*ElementRC(row3, col1);
   det -= ElementRC(row1, col2)*ElementRC(row3, col3)*ElementRC(row2, col1);
   det -= ElementRC(row1, col1)*ElementRC(row2, col3)*ElementRC(row3, col2);
   return det;
}


/************************************
* m3dMATR::MakeEuler
*
************************************/
void m3dMATR::MakeEuler (float angle_x, float angle_y, float angle_z)
{
   float A       = m3dCos(m3dDeg2Rad(angle_x));
   float B       = m3dSin(m3dDeg2Rad(angle_x));
   float C       = m3dCos(m3dDeg2Rad(angle_y));
   float D       = m3dSin(m3dDeg2Rad(angle_y));
   float E       = m3dCos(m3dDeg2Rad(angle_z));
   float F       = m3dSin(m3dDeg2Rad(angle_z));
   float AD      =   A * D;
   float BD      =   B * D;

   Element(0) =   C * E;            //0,0
   Element(4)  =  -C * F;           //0,1
   Element(8)  =   D;               //0,2
   Element(1)  =  BD * E + A * F;   //1,0
   Element(5)  = -BD * F + A * E;   //1,1
   Element(9)  =  -B * C;           //1,2
   Element(2)  = -AD * E + B * F;   //2,0
   Element(6)  =  AD * F + B * E;   //2,1
   Element(10) =   A * C;           //2,2

   Element(3)  =  Element(7) = Element(11) = Element(12) = Element(13) = Element(14) = 0;
   Element(15) =  1;
}


///////////////////////////////
//
// 3D Matrix (double) functions
//

void m3dMATRd::Transpose(void)
{
   m3dSwap(ElementRC(0, 1), ElementRC(1, 0));
   m3dSwap(ElementRC(0, 2), ElementRC(2, 0));
   m3dSwap(ElementRC(0, 3), ElementRC(3, 0));
   m3dSwap(ElementRC(1, 2), ElementRC(2, 1));
   m3dSwap(ElementRC(1, 3), ElementRC(3, 1));
   m3dSwap(ElementRC(2, 3), ElementRC(3, 2));
   return;
}

void m3dMATRd::Transpose(const m3dMATRd* __RESTRICT pSrc, m3dMATRd* __RESTRICT pDst)
{
   pDst->ElementRC(0, 0) = pSrc->ElementRC(0, 0);
   pDst->ElementRC(0, 1) = pSrc->ElementRC(1, 0);
   pDst->ElementRC(0, 2) = pSrc->ElementRC(2, 0);
   pDst->ElementRC(0, 3) = pSrc->ElementRC(3, 0);

   pDst->ElementRC(1, 0) = pSrc->ElementRC(0, 1);
   pDst->ElementRC(1, 1) = pSrc->ElementRC(1, 1);
   pDst->ElementRC(1, 2) = pSrc->ElementRC(2, 1);
   pDst->ElementRC(1, 3) = pSrc->ElementRC(3, 1);

   pDst->ElementRC(2, 0) = pSrc->ElementRC(0, 2);
   pDst->ElementRC(2, 1) = pSrc->ElementRC(1, 2);
   pDst->ElementRC(2, 2) = pSrc->ElementRC(2, 2);
   pDst->ElementRC(2, 3) = pSrc->ElementRC(3, 2);

   pDst->ElementRC(3, 0) = pSrc->ElementRC(0, 3);
   pDst->ElementRC(3, 1) = pSrc->ElementRC(1, 3);
   pDst->ElementRC(3, 2) = pSrc->ElementRC(2, 3);
   pDst->ElementRC(3, 3) = pSrc->ElementRC(3, 3);
}

void m3dMultiplyMatrAccuracy(const m3dMATRd * __RESTRICT a, const m3dMATRd * __RESTRICT b, m3dMATRd * __RESTRICT c)
{
   int       row, col;
   m3dMATRd   tmp, * __RESTRICT _c;

   _c = c;
   if (a == c || b == c) {
      _c  = &tmp;
   }
   for (row = 0; row < 4; row++)  {
      for (col = 0; col < 4; col++)  {
         m3dMatrElementRC(_c,row,col) = 
            m3dMatrElementRC(a,row,0) * m3dMatrElementRC(b,0,col) + 
            m3dMatrElementRC(a,row,1) * m3dMatrElementRC(b,1,col) + 
            m3dMatrElementRC(a,row,2) * m3dMatrElementRC(b,2,col) + 
            m3dMatrElementRC(a,row,3) * m3dMatrElementRC(b,3,col);
      }
   }
   if (a == c || b == c) {
      *c = tmp;
   }

   return;
}

BOOL m3dMATRd::Invert(m3dMATRd *destMatr) const
{
   // 84+4+16 = 104 multiplications
   //          1 division
   double det, invDet;

   // 2x2 sub-determinants required to calculate 4x4 determinant
   double det2_01_01 = m[0][0] * m[1][1] - m[0][1] * m[1][0];
   double det2_01_02 = m[0][0] * m[1][2] - m[0][2] * m[1][0];
   double det2_01_03 = m[0][0] * m[1][3] - m[0][3] * m[1][0];
   double det2_01_12 = m[0][1] * m[1][2] - m[0][2] * m[1][1];
   double det2_01_13 = m[0][1] * m[1][3] - m[0][3] * m[1][1];
   double det2_01_23 = m[0][2] * m[1][3] - m[0][3] * m[1][2];

   // 3x3 sub-determinants required to calculate 4x4 determinant
   double det3_201_012 = m[2][0] * det2_01_12 - m[2][1] * det2_01_02 + m[2][2] * det2_01_01;
   double det3_201_013 = m[2][0] * det2_01_13 - m[2][1] * det2_01_03 + m[2][3] * det2_01_01;
   double det3_201_023 = m[2][0] * det2_01_23 - m[2][2] * det2_01_03 + m[2][3] * det2_01_02;
   double det3_201_123 = m[2][1] * det2_01_23 - m[2][2] * det2_01_13 + m[2][3] * det2_01_12;

   det = ( - det3_201_123 * m[3][0] + det3_201_023 * m[3][1] - det3_201_013 * m[3][2] + det3_201_012 * m[3][3] );

   if ( fabs( det ) < MATRIX_INVERSE_EPSILON ) {
      return FALSE;
   }

   invDet = 1.0 / det;

   // remaining 2x2 sub-determinants
   double det2_03_01 = m[0][0] * m[3][1] - m[0][1] * m[3][0];
   double det2_03_02 = m[0][0] * m[3][2] - m[0][2] * m[3][0];
   double det2_03_03 = m[0][0] * m[3][3] - m[0][3] * m[3][0];
   double det2_03_12 = m[0][1] * m[3][2] - m[0][2] * m[3][1];
   double det2_03_13 = m[0][1] * m[3][3] - m[0][3] * m[3][1];
   double det2_03_23 = m[0][2] * m[3][3] - m[0][3] * m[3][2];

   double det2_13_01 = m[1][0] * m[3][1] - m[1][1] * m[3][0];
   double det2_13_02 = m[1][0] * m[3][2] - m[1][2] * m[3][0];
   double det2_13_03 = m[1][0] * m[3][3] - m[1][3] * m[3][0];
   double det2_13_12 = m[1][1] * m[3][2] - m[1][2] * m[3][1];
   double det2_13_13 = m[1][1] * m[3][3] - m[1][3] * m[3][1];
   double det2_13_23 = m[1][2] * m[3][3] - m[1][3] * m[3][2];

   // remaining 3x3 sub-determinants
   double det3_203_012 = m[2][0] * det2_03_12 - m[2][1] * det2_03_02 + m[2][2] * det2_03_01;
   double det3_203_013 = m[2][0] * det2_03_13 - m[2][1] * det2_03_03 + m[2][3] * det2_03_01;
   double det3_203_023 = m[2][0] * det2_03_23 - m[2][2] * det2_03_03 + m[2][3] * det2_03_02;
   double det3_203_123 = m[2][1] * det2_03_23 - m[2][2] * det2_03_13 + m[2][3] * det2_03_12;

   double det3_213_012 = m[2][0] * det2_13_12 - m[2][1] * det2_13_02 + m[2][2] * det2_13_01;
   double det3_213_013 = m[2][0] * det2_13_13 - m[2][1] * det2_13_03 + m[2][3] * det2_13_01;
   double det3_213_023 = m[2][0] * det2_13_23 - m[2][2] * det2_13_03 + m[2][3] * det2_13_02;
   double det3_213_123 = m[2][1] * det2_13_23 - m[2][2] * det2_13_13 + m[2][3] * det2_13_12;

   double det3_301_012 = m[3][0] * det2_01_12 - m[3][1] * det2_01_02 + m[3][2] * det2_01_01;
   double det3_301_013 = m[3][0] * det2_01_13 - m[3][1] * det2_01_03 + m[3][3] * det2_01_01;
   double det3_301_023 = m[3][0] * det2_01_23 - m[3][2] * det2_01_03 + m[3][3] * det2_01_02;
   double det3_301_123 = m[3][1] * det2_01_23 - m[3][2] * det2_01_13 + m[3][3] * det2_01_12;

   destMatr->m[0][0] = double(- det3_213_123 * invDet);
   destMatr->m[1][0] = double(+ det3_213_023 * invDet);
   destMatr->m[2][0] = double(- det3_213_013 * invDet);
   destMatr->m[3][0] = double(+ det3_213_012 * invDet);

   destMatr->m[0][1] = double(+ det3_203_123 * invDet);
   destMatr->m[1][1] = double(- det3_203_023 * invDet);
   destMatr->m[2][1] = double(+ det3_203_013 * invDet);
   destMatr->m[3][1] = double(- det3_203_012 * invDet);

   destMatr->m[0][2] = double(+ det3_301_123 * invDet);
   destMatr->m[1][2] = double(- det3_301_023 * invDet);
   destMatr->m[2][2] = double(+ det3_301_013 * invDet);
   destMatr->m[3][2] = double(- det3_301_012 * invDet);

   destMatr->m[0][3] = double(- det3_201_123 * invDet);
   destMatr->m[1][3] = double(+ det3_201_023 * invDet);
   destMatr->m[2][3] = double(- det3_201_013 * invDet);
   destMatr->m[3][3] = double(+ det3_201_012 * invDet);

   return TRUE;
}

//
// End-of-file M3D_MATR.CPP
//

